← Back to branch summary

~ubuntu-branches/ubuntu/trusty/blender/trusty

« back to all changes in this revision

Viewing changes to extern/eltopo/common/collisionqueries.cpp

  • Committer: Package Import Robot
  • Author(s): Jeremy Bicha
  • Date: 2013-03-06 12:08:47 UTC
  • mfrom: (1.5.1) (14.1.8 experimental)
  • Revision ID: package-import@ubuntu.com-20130306120847-frjfaryb2zrotwcg
Tags: 2.66a-1ubuntu1
https://launchpad.net/bugs/1076930
https://launchpad.net/bugs/1089256
https://launchpad.net/bugs/1052743
https://launchpad.net/bugs/999024
* Resynchronize with Debian (LP: #1076930, #1089256, #1052743, #999024,
  #1122888, #1147084)
* debian/control:
  - Lower build-depends on libavcodec-dev since we're not
    doing the libav9 transition in Ubuntu yet

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/eltopo/common/collisionqueries.cpp
1
 
#include <collisionqueries.h>
2
 
#include <commonoptions.h>
3
 
 
4
 
 
5
 
 
6
 
void check_point_edge_proximity(bool update, const Vec3d &x0, const Vec3d &x1, const Vec3d &x2,
7
 
                                double &distance)
8
 
{
9
 
    Vec3d dx(x2-x1);
10
 
    double m2=mag2(dx);
11
 
    // find parameter value of closest point on segment
12
 
    double s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
13
 
    // and find the distance
14
 
    if(update){
15
 
        distance=min(distance, dist(x0,s*x1+(1-s)*x2));
16
 
    }else{
17
 
        distance=dist(x0,s*x1+(1-s)*x2);
18
 
    }
19
 
}
20
 
 
21
 
// normal is from 1-2 towards 0, unless normal_multiplier<0
22
 
void check_point_edge_proximity(bool update, const Vec3d &x0, const Vec3d &x1, const Vec3d &x2,
23
 
                                double &distance, double &s, Vec3d &normal, double normal_multiplier)
24
 
{
25
 
    Vec3d dx(x2-x1);
26
 
    double m2=mag2(dx);
27
 
    if(update){
28
 
        // find parameter value of closest point on segment
29
 
        double this_s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
30
 
        // and find the distance
31
 
        Vec3d this_normal=x0-(this_s*x1+(1-this_s)*x2);
32
 
        double this_distance=mag(this_normal);
33
 
        if(this_distance<distance){
34
 
            s=this_s;
35
 
            distance=this_distance;
36
 
            normal=(normal_multiplier/(this_distance+1e-30))*this_normal;
37
 
        }
38
 
    }else{
39
 
        // find parameter value of closest point on segment
40
 
        s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
41
 
        // and find the distance
42
 
        normal=x0-(s*x1+(1-s)*x2);
43
 
        distance=mag(normal);
44
 
        normal*=normal_multiplier/(distance+1e-30);
45
 
    }
46
 
}
47
 
 
48
 
void check_point_edge_proximity( bool update, const Vec2d &x0, const Vec2d &x1, const Vec2d &x2,
49
 
                                double &distance)
50
 
{
51
 
    Vec2d dx(x2-x1);
52
 
    double m2=mag2(dx);
53
 
    // find parameter value of closest point on segment
54
 
    double s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
55
 
    // and find the distance
56
 
    if(update){
57
 
        distance=min(distance, dist(x0,s*x1+(1-s)*x2));
58
 
    }else{
59
 
        distance=dist(x0, s*x1+(1-s)*x2);
60
 
    }
61
 
}
62
 
 
63
 
// normal is from 1-2 towards 0, unless normal_multiplier<0
64
 
void check_point_edge_proximity(bool update, const Vec2d &x0, const Vec2d &x1, const Vec2d &x2,
65
 
                                double &distance, double &s, Vec2d &normal, double normal_multiplier)
66
 
{
67
 
    Vec2d dx(x2-x1);
68
 
    double m2=mag2(dx);
69
 
    if(update){
70
 
        // find parameter value of closest point on segment
71
 
        double this_s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
72
 
        // and find the distance
73
 
        Vec2d this_normal=x0-(this_s*x1+(1-this_s)*x2);
74
 
        double this_distance=mag(this_normal);
75
 
        if(this_distance<distance){
76
 
            s=this_s;
77
 
            distance=this_distance;
78
 
            normal=(normal_multiplier/(this_distance+1e-30))*this_normal;
79
 
        }
80
 
    }else{
81
 
        // find parameter value of closest point on segment
82
 
        s=clamp(dot(x2-x0, dx)/m2, 0., 1.);
83
 
        // and find the distance
84
 
        normal=x0-(s*x1+(1-s)*x2);
85
 
        distance=mag(normal);
86
 
        if ( distance < 1e-10 )
87
 
        {
88
 
            normal = normalized(perp(x2 - x1));
89
 
            return;
90
 
        }
91
 
        
92
 
        normal*=normal_multiplier/(distance+1e-30);
93
 
    }
94
 
}
95
 
 
96
 
void check_edge_edge_proximity(const Vec3d &x0, const Vec3d &x1, const Vec3d &x2, const Vec3d &x3, double &distance)
97
 
{
98
 
    // let's do it the QR way for added robustness
99
 
    Vec3d x01=x0-x1;
100
 
    double r00=mag(x01)+1e-30;
101
 
    x01/=r00;
102
 
    Vec3d x32=x3-x2;
103
 
    double r01=dot(x32,x01);
104
 
    x32-=r01*x01;
105
 
    double r11=mag(x32)+1e-30;
106
 
    x32/=r11;
107
 
    Vec3d x31=x3-x1;
108
 
    double s2=dot(x32,x31)/r11;
109
 
    double s0=(dot(x01,x31)-r01*s2)/r00;
110
 
    // check if we're in range
111
 
    if(s0<0){
112
 
        if(s2<0){
113
 
            // check both x1 against 2-3 and 3 against 0-1
114
 
            check_point_edge_proximity(false, x1, x2, x3, distance);
115
 
            check_point_edge_proximity(true, x3, x0, x1, distance);
116
 
        }else if(s2>1){
117
 
            // check both x1 against 2-3 and 2 against 0-1
118
 
            check_point_edge_proximity(false, x1, x2, x3, distance);
119
 
            check_point_edge_proximity(true, x2, x0, x1, distance);
120
 
        }else{
121
 
            s0=0;
122
 
            // check x1 against 2-3
123
 
            check_point_edge_proximity(false, x1, x2, x3, distance);
124
 
        }
125
 
    }else if(s0>1){
126
 
        if(s2<0){
127
 
            // check both x0 against 2-3 and 3 against 0-1
128
 
            check_point_edge_proximity(false, x0, x2, x3, distance);
129
 
            check_point_edge_proximity(true, x3, x0, x1, distance);
130
 
        }else if(s2>1){
131
 
            // check both x0 against 2-3 and 2 against 0-1
132
 
            check_point_edge_proximity(false, x0, x2, x3, distance);
133
 
            check_point_edge_proximity(true, x2, x0, x1, distance);
134
 
        }else{
135
 
            s0=1;
136
 
            // check x0 against 2-3
137
 
            check_point_edge_proximity(false, x0, x2, x3, distance);
138
 
        }
139
 
    }else{
140
 
        if(s2<0){
141
 
            s2=0;
142
 
            // check x3 against 0-1
143
 
            check_point_edge_proximity(false, x3, x0, x1, distance);
144
 
        }else if(s2>1){
145
 
            s2=1;
146
 
            // check x2 against 0-1
147
 
            check_point_edge_proximity(false, x2, x0, x1, distance);
148
 
        }else{ // we already got the closest points!
149
 
            distance=dist(s2*x2+(1-s2)*x3, s0*x0+(1-s0)*x1);
150
 
        }
151
 
    }
152
 
}
153
 
 
154
 
// find distance between 0-1 and 2-3, with barycentric coordinates for closest points, and
155
 
// a normal that points from 0-1 towards 2-3 (unreliable if distance==0 or very small)
156
 
void check_edge_edge_proximity(const Vec3d &x0, const Vec3d &x1, const Vec3d &x2, const Vec3d &x3,
157
 
                               double &distance, double &s0, double &s2, Vec3d &normal)
158
 
{
159
 
    // let's do it the QR way for added robustness
160
 
    Vec3d x01=x0-x1;
161
 
    double r00=mag(x01)+1e-30;
162
 
    x01/=r00;
163
 
    Vec3d x32=x3-x2;
164
 
    double r01=dot(x32,x01);
165
 
    x32-=r01*x01;
166
 
    double r11=mag(x32)+1e-30;
167
 
    x32/=r11;
168
 
    Vec3d x31=x3-x1;
169
 
    s2=dot(x32,x31)/r11;
170
 
    s0=(dot(x01,x31)-r01*s2)/r00;
171
 
    // check if we're in range
172
 
    if(s0<0){
173
 
        if(s2<0){
174
 
            // check both x1 against 2-3 and 3 against 0-1
175
 
            check_point_edge_proximity(false, x1, x2, x3, distance, s2, normal, 1.);
176
 
            check_point_edge_proximity(true, x3, x0, x1, distance, s0, normal, -1.);
177
 
        }else if(s2>1){
178
 
            // check both x1 against 2-3 and 2 against 0-1
179
 
            check_point_edge_proximity(false, x1, x2, x3, distance, s2, normal, 1.);
180
 
            check_point_edge_proximity(true, x2, x0, x1, distance, s0, normal, -1.);
181
 
        }else{
182
 
            s0=0;
183
 
            // check x1 against 2-3
184
 
            check_point_edge_proximity(false, x1, x2, x3, distance, s2, normal, 1.);
185
 
        }
186
 
    }else if(s0>1){
187
 
        if(s2<0){
188
 
            // check both x0 against 2-3 and 3 against 0-1
189
 
            check_point_edge_proximity(false, x0, x2, x3, distance, s2, normal, 1.);
190
 
            check_point_edge_proximity(true, x3, x0, x1, distance, s0, normal, -1.);
191
 
        }else if(s2>1){
192
 
            // check both x0 against 2-3 and 2 against 0-1
193
 
            check_point_edge_proximity(false, x0, x2, x3, distance, s2, normal, 1.);
194
 
            check_point_edge_proximity(true, x2, x0, x1, distance, s0, normal, -1.);
195
 
        }else{
196
 
            s0=1;
197
 
            // check x0 against 2-3
198
 
            check_point_edge_proximity(false, x0, x2, x3, distance, s2, normal, 1.);
199
 
        }
200
 
    }else{
201
 
        if(s2<0){
202
 
            s2=0;
203
 
            // check x3 against 0-1
204
 
            check_point_edge_proximity(false, x3, x0, x1, distance, s0, normal, -1.);
205
 
        }else if(s2>1){
206
 
            s2=1;
207
 
            // check x2 against 0-1
208
 
            check_point_edge_proximity(false, x2, x0, x1, distance, s0, normal, -1.);
209
 
        }else{ // we already got the closest points!
210
 
            normal=(s0*x0+(1-s0)*x1)-(s2*x2+(1-s2)*x3);
211
 
            distance=mag(normal);
212
 
            if(distance>0) normal/=distance;
213
 
            else{
214
 
                normal=cross(x1-x0, x3-x2);
215
 
                normal/=mag(normal)+1e-300;
216
 
            }
217
 
        }
218
 
    }
219
 
}
220
 
 
221
 
void check_point_triangle_proximity(const Vec3d &x0, const Vec3d &x1, const Vec3d &x2, const Vec3d &x3,
222
 
                                    double &distance)
223
 
{
224
 
    // do it the QR way for added robustness
225
 
    Vec3d x13=x1-x3;
226
 
    double r00=mag(x13)+1e-30;
227
 
    x13/=r00;
228
 
    Vec3d x23=x2-x3;
229
 
    double r01=dot(x23,x13);
230
 
    x23-=r01*x13;
231
 
    double r11=mag(x23)+1e-30;
232
 
    x23/=r11;
233
 
    Vec3d x03=x0-x3;
234
 
    double s2=dot(x23,x03)/r11;
235
 
    double s1=(dot(x13,x03)-r01*s2)/r00;
236
 
    double s3=1-s1-s2;
237
 
    // check if we are in range
238
 
    if(s1>=0 && s2>=0 && s3>=0){
239
 
        distance=dist(x0, s1*x1+s2*x2+s3*x3);
240
 
    }else{
241
 
        if(s1>0){ // rules out edge 2-3
242
 
            check_point_edge_proximity(false, x0, x1, x2, distance);
243
 
            check_point_edge_proximity(true, x0, x1, x3, distance);
244
 
        }else if(s2>0){ // rules out edge 1-3
245
 
            check_point_edge_proximity(false, x0, x1, x2, distance);
246
 
            check_point_edge_proximity(true, x0, x2, x3, distance);
247
 
        }else{ // s3>0: rules out edge 1-2
248
 
            check_point_edge_proximity(false, x0, x2, x3, distance);
249
 
            check_point_edge_proximity(true, x0, x1, x3, distance);
250
 
        }
251
 
    }
252
 
}
253
 
 
254
 
// find distance between 0 and 1-2-3, with barycentric coordinates for closest point, and
255
 
// a normal that points from 1-2-3 towards 0 (unreliable if distance==0 or very small)
256
 
void check_point_triangle_proximity(const Vec3d &x0, const Vec3d &x1, const Vec3d &x2, const Vec3d &x3,
257
 
                                    double &distance, double &s1, double &s2, double &s3, Vec3d &normal)
258
 
{
259
 
    // do it the QR way for added robustness
260
 
    Vec3d x13=x1-x3;
261
 
    double r00=mag(x13)+1e-30;
262
 
    x13/=r00;
263
 
    Vec3d x23=x2-x3;
264
 
    double r01=dot(x23,x13);
265
 
    x23-=r01*x13;
266
 
    double r11=mag(x23)+1e-30;
267
 
    x23/=r11;
268
 
    Vec3d x03=x0-x3;
269
 
    s2=dot(x23,x03)/r11;
270
 
    s1=(dot(x13,x03)-r01*s2)/r00;
271
 
    s3=1-s1-s2;
272
 
    // check if we are in range
273
 
    if(s1>=0 && s2>=0 && s3>=0){
274
 
        normal=x0-(s1*x1+s2*x2+s3*x3);
275
 
        distance=mag(normal);
276
 
        if(distance>0) normal/=distance;
277
 
        else{
278
 
            normal=cross(x2-x1, x3-x1);
279
 
            normal/=mag(normal)+1e-300;
280
 
        }
281
 
    }else{
282
 
        double s, d;
283
 
        if(s1>0){ // rules out edge 2-3
284
 
            check_point_edge_proximity(false, x0, x1, x2, distance, s, normal, 1.);
285
 
            s1=s; s2=1-s; s3=0; d=distance;
286
 
            check_point_edge_proximity(true, x0, x1, x3, distance, s, normal, 1.);
287
 
            if(distance<d){
288
 
                s1=s; s2=0; s3=1-s;
289
 
            }
290
 
        }else if(s2>0){ // rules out edge 1-3
291
 
            check_point_edge_proximity(false, x0, x1, x2, distance, s, normal, 1.);
292
 
            s1=s; s2=1-s; s3=0; d=distance;
293
 
            check_point_edge_proximity(true, x0, x2, x3, distance, s, normal, 1.);
294
 
            if(distance<d){
295
 
                s1=0; s2=s; s3=1-s; d=distance;
296
 
            }
297
 
        }else{ // s3>0: rules out edge 1-2
298
 
            check_point_edge_proximity(false, x0, x2, x3, distance, s, normal, 1.);
299
 
            s1=0; s2=s; s3=1-s; d=distance;
300
 
            check_point_edge_proximity(true, x0, x1, x3, distance, s, normal, 1.);
301
 
            if(distance<d){
302
 
                s1=s; s2=0; s3=1-s;
303
 
            }
304
 
        }
305
 
    }
306
 
}
307
 
 
308
 
double signed_volume(const Vec3d &x0, const Vec3d &x1, const Vec3d &x2, const Vec3d &x3)
309
 
{
310
 
    // Equivalent to triple(x1-x0, x2-x0, x3-x0), six times the signed volume of the tetrahedron.
311
 
    // But, for robustness, we want the result (up to sign) to be independent of the ordering.
312
 
    // And want it as accurate as possible...
313
 
    // But all that stuff is hard, so let's just use the common assumption that all coordinates are >0,
314
 
    // and do something reasonably accurate in fp.
315
 
    
316
 
    // This formula does almost four times too much multiplication, but if the coordinates are non-negative
317
 
    // it suffers in a minimal way from cancellation error.
318
 
    return ( x0[0]*(x1[1]*x3[2]+x3[1]*x2[2]+x2[1]*x1[2])
319
 
            +x1[0]*(x2[1]*x3[2]+x3[1]*x0[2]+x0[1]*x2[2])
320
 
            +x2[0]*(x3[1]*x1[2]+x1[1]*x0[2]+x0[1]*x3[2])
321
 
            +x3[0]*(x1[1]*x2[2]+x2[1]*x0[2]+x0[1]*x1[2]) )
322
 
    
323
 
    - ( x0[0]*(x2[1]*x3[2]+x3[1]*x1[2]+x1[1]*x2[2])
324
 
       +x1[0]*(x3[1]*x2[2]+x2[1]*x0[2]+x0[1]*x3[2])
325
 
       +x2[0]*(x1[1]*x3[2]+x3[1]*x0[2]+x0[1]*x1[2])
326
 
       +x3[0]*(x2[1]*x1[2]+x1[1]*x0[2]+x0[1]*x2[2]) );
327
 
}
328