← Back to branch summary

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

« back to all changes in this revision

Viewing changes to extern/eltopo/common/ccd_wrapper.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/ccd_wrapper.cpp
1
 
// ---------------------------------------------------------
2
 
//
3
 
//  ccd_wrapper.cpp
4
 
//  Tyson Brochu 2009
5
 
//
6
 
//  Tunicate-based implementation of collision and intersection queries.  (See Robert Bridson's "Tunicate" library.)
7
 
//
8
 
// ---------------------------------------------------------
9
 
 
10
 
#include <bfstream.h>
11
 
#include <ccd_defs.h>
12
 
#include <ccd_wrapper.h>
13
 
#include <collisionqueries.h>
14
 
#include <tunicate.h>
15
 
#include <vec.h>
16
 
 
17
 
bool tunicate_verbose = false;
18
 
 
19
 
#if defined(_WIN32) && !defined(FREE_WINDOWS)
20
 
#define random() rand() // not sure if this define is valid
21
 
#define _Ios_Fmtflags ios::fmtflags
22
 
#endif
23
 
 
24
 
 
25
 
#ifdef USE_TUNICATE_CCD
26
 
 
27
 
 
28
 
// --------------------------------------------------------------------------------------------------
29
 
// Local functions
30
 
// --------------------------------------------------------------------------------------------------
31
 
 
32
 
namespace {
33
 
    
34
 
    bool tunicate_point_segment_collision( const Vec2d& x0, const Vec2d& xnew0, size_t index0,
35
 
                                          const Vec2d& x1, const Vec2d& xnew1, size_t index1,
36
 
                                          const Vec2d& x2, const Vec2d& xnew2, size_t index2 );
37
 
    
38
 
    bool tunicate_point_segment_collision(const Vec2d& x0, const Vec2d& xnew0, size_t index0,
39
 
                                          const Vec2d& x1, const Vec2d& xnew1, size_t index1,
40
 
                                          const Vec2d& x2, const Vec2d& xnew2, size_t index2,
41
 
                                          double& edge_alpha, Vec2d& normal, double& time, double& relative_normal_displacement );
42
 
    
43
 
    bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
44
 
                                           const Vec3d& x1, const Vec3d& xnew1, size_t index1,
45
 
                                           const Vec3d& x2, const Vec3d& xnew2, size_t index2,
46
 
                                           const Vec3d& x3, const Vec3d& xnew3, size_t index3,
47
 
                                           double& bary1, double& bary2, double& bary3,
48
 
                                           Vec3d& normal,
49
 
                                           double& t,
50
 
                                           double& relative_normal_displacement,
51
 
                                           bool verbose );
52
 
    
53
 
    bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
54
 
                                           const Vec3d& x1, const Vec3d& xnew1, size_t index1,
55
 
                                           const Vec3d& x2, const Vec3d& xnew2, size_t index2,
56
 
                                           const Vec3d& x3, const Vec3d& xnew3, size_t index3 );
57
 
    
58
 
    bool tunicate_segment_segment_collision( const Vec3d& x0, const Vec3d& xnew0, size_t index0,
59
 
                                            const Vec3d& x1, const Vec3d& xnew1,  size_t index1,
60
 
                                            const Vec3d& x2, const Vec3d& xnew2,  size_t index2,
61
 
                                            const Vec3d& x3, const Vec3d& xnew3,  size_t index3,
62
 
                                            double& bary0, double& bary2,
63
 
                                            Vec3d& normal,
64
 
                                            double& t,
65
 
                                            double& relative_normal_displacement,
66
 
                                            bool verbose );
67
 
    
68
 
    bool tunicate_segment_segment_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
69
 
                                            const Vec3d& x1, const Vec3d& xnew1, size_t index1,
70
 
                                            const Vec3d& x2, const Vec3d& xnew2, size_t index2,
71
 
                                            const Vec3d& x3, const Vec3d& xnew3, size_t index3);
72
 
    
73
 
    // --------------------------------------------------------------------------------------------------------------
74
 
    
75
 
    Vec2d get_normal( const Vec2d& v )
76
 
    {
77
 
        Vec2d p = perp( v );
78
 
        double m = mag(p);
79
 
        if ( m > 0.0 ) { return p / m; }
80
 
        
81
 
        // degenerate, pick a random unit vector:
82
 
        p[0] = random()%2 ? -0.707106781186548 : 0.707106781186548;
83
 
        p[1] = random()%2 ? -0.707106781186548 : 0.707106781186548;
84
 
        
85
 
        return p;
86
 
    }
87
 
    
88
 
    // --------------------------------------------------------------------------------------------------------------
89
 
    
90
 
    Vec3d get_normal(const Vec3d& u, const Vec3d& v)
91
 
    {
92
 
        Vec3d c=cross(u,v);
93
 
        double m=mag(c);
94
 
        if(m) return c/m;
95
 
        
96
 
        // degenerate case: either u and v are parallel, or at least one is zero; pick an arbitrary orthogonal vector
97
 
        if(mag2(u)>=mag2(v)){
98
 
            if(std::fabs(u[0])>=std::fabs(u[1]) && std::fabs(u[0])>=std::fabs(u[2]))
99
 
                c=Vec3d(-u[1]-u[2], u[0], u[0]);
100
 
            else if(std::fabs(u[1])>=std::fabs(u[2]))
101
 
                c=Vec3d(u[1], -u[0]-u[2], u[1]);
102
 
            else
103
 
                c=Vec3d(u[2], u[2], -u[0]-u[1]);
104
 
        }else{
105
 
            if(std::fabs(v[0])>=std::fabs(v[1]) && std::fabs(v[0])>=std::fabs(v[2]))
106
 
                c=Vec3d(-v[1]-v[2], v[0], v[0]);
107
 
            else if(std::fabs(v[1])>=std::fabs(v[2]))
108
 
                c=Vec3d(v[1], -v[0]-v[2], v[1]);
109
 
            else
110
 
                c=Vec3d(v[2], v[2], -v[0]-v[1]);
111
 
        }
112
 
        m=mag(c);
113
 
        if(m) return c/m;
114
 
        
115
 
        // really degenerate case: u and v are both zero vectors; pick a random unit-length vector
116
 
        c[0]=random()%2 ? -0.577350269189626 : 0.577350269189626;
117
 
        c[1]=random()%2 ? -0.577350269189626 : 0.577350269189626;
118
 
        c[2]=random()%2 ? -0.577350269189626 : 0.577350269189626;
119
 
        return c;
120
 
        
121
 
    }
122
 
    
123
 
    
124
 
    // --------------------------------------------------------------------------------------------------------------
125
 
    
126
 
    bool tunicate_point_segment_collision( const Vec2d& x0, const Vec2d& xnew0, size_t,
127
 
                                          const Vec2d& x1, const Vec2d& xnew1, size_t index1,
128
 
                                          const Vec2d& x2, const Vec2d& xnew2, size_t index2 )
129
 
    {
130
 
        assert( index1 < index2 );
131
 
        
132
 
        const int segment_triangle_test = 2;
133
 
        
134
 
        double p0[3] = { x0[0], x0[1], 0.0 };
135
 
        double pnew0[3] = { xnew0[0], xnew0[1], 1.0 };
136
 
        double p1[3] = { x1[0], x1[1], 0.0 };
137
 
        double pnew1[3] = { xnew1[0], xnew1[1], 1.0 };
138
 
        double p2[3] = { x2[0], x2[1], 0.0 };
139
 
        double pnew2[3] = { xnew2[0], xnew2[1], 1.0 };
140
 
        
141
 
        double bary[5];
142
 
        
143
 
        bool intersections[2] = { false, false };
144
 
        
145
 
        if ( simplex_intersection3d( segment_triangle_test,
146
 
                                    p0, pnew0, p1, p2, pnew2,
147
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] ) )
148
 
        {
149
 
            intersections[0] = true;
150
 
        }
151
 
        
152
 
        if ( simplex_intersection3d( segment_triangle_test,
153
 
                                    p0, pnew0, p1, pnew1, pnew2,
154
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] ) )
155
 
        {
156
 
            intersections[1] = true;
157
 
        }
158
 
        
159
 
        return ( intersections[0] ^ intersections[1] );  
160
 
        
161
 
    }
162
 
    
163
 
    // --------------------------------------------------------------------------------------------------------------
164
 
    
165
 
    bool tunicate_point_segment_collision(const Vec2d& x0, const Vec2d& xnew0, size_t,
166
 
                                          const Vec2d& x1, const Vec2d& xnew1, size_t index1,
167
 
                                          const Vec2d& x2, const Vec2d& xnew2, size_t index2,
168
 
                                          double& edge_alpha, Vec2d& normal, double& time, double& relative_normal_displacement )
169
 
    {
170
 
        
171
 
        assert( index1 < index2 );
172
 
        
173
 
        const int segment_triangle_test = 2;
174
 
        
175
 
        double p0[3] = { x0[0], x0[1], 0.0 };
176
 
        double pnew0[3] = { xnew0[0], xnew0[1], 1.0 };
177
 
        double p1[3] = { x1[0], x1[1], 0.0 };
178
 
        double pnew1[3] = { xnew1[0], xnew1[1], 1.0 };
179
 
        double p2[3] = { x2[0], x2[1], 0.0 };
180
 
        double pnew2[3] = { xnew2[0], xnew2[1], 1.0 };
181
 
        
182
 
        double bary[5];   
183
 
        bool intersections[2] = { false, false };
184
 
        
185
 
        if ( simplex_intersection3d( segment_triangle_test,
186
 
                                    p0, pnew0, p1, p2, pnew2,
187
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] ) )
188
 
        {
189
 
            intersections[0] = true;      
190
 
            edge_alpha=0;     // bary1 = 0, bary2 = 1
191
 
            time=bary[1];
192
 
            normal = get_normal( x2-x1 );
193
 
            relative_normal_displacement = dot( normal, (xnew0-x0)-(xnew2-x2) );
194
 
        }
195
 
        
196
 
        if ( simplex_intersection3d( segment_triangle_test,
197
 
                                    p0, pnew0, p1, pnew1, pnew2,
198
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] ) )
199
 
        {
200
 
            intersections[1] = true;
201
 
            edge_alpha=1;     // bary1 = 1, bary2 = 0
202
 
            time=bary[1];
203
 
            normal = get_normal( xnew2-xnew1 );
204
 
            relative_normal_displacement = dot( normal, (xnew0-x0)-(xnew1-x1) );
205
 
        }
206
 
        
207
 
        return ( intersections[0] ^ intersections[1] );  
208
 
        
209
 
    }
210
 
    
211
 
    
212
 
    // --------------------------------------------------------------------------------------------------------------
213
 
    
214
 
    bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, size_t,
215
 
                                           const Vec3d& x1, const Vec3d& xnew1, size_t index1,
216
 
                                           const Vec3d& x2, const Vec3d& xnew2, size_t index2,
217
 
                                           const Vec3d& x3, const Vec3d& xnew3, size_t index3 )
218
 
    {
219
 
        
220
 
        assert( index1 < index2 && index2 < index3 );
221
 
        
222
 
        const int segment_tetrahedron_test = 2;
223
 
        
224
 
        double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
225
 
        double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
226
 
        double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
227
 
        double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
228
 
        double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
229
 
        double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
230
 
        double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
231
 
        double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
232
 
        
233
 
        
234
 
        size_t num_intersections = 0;
235
 
        
236
 
        double bary[6];
237
 
        
238
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
239
 
                                    p0, pnew0, p1, p2, p3, pnew3,
240
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
241
 
        {
242
 
            ++num_intersections;
243
 
        }
244
 
        
245
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
246
 
                                    p0, pnew0, p1, p2, pnew2, pnew3,
247
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
248
 
        {
249
 
            ++num_intersections;
250
 
        }
251
 
        
252
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
253
 
                                    p0, pnew0, p1, pnew1, pnew2, pnew3,
254
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
255
 
        {
256
 
            ++num_intersections;
257
 
        }
258
 
        
259
 
        if ( num_intersections == 0 || num_intersections == 2 )
260
 
        {
261
 
            return false;
262
 
        }
263
 
        
264
 
        return true;
265
 
        
266
 
    }
267
 
    
268
 
    
269
 
    // --------------------------------------------------------------------------------------------------------------
270
 
    
271
 
    bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, size_t,
272
 
                                           const Vec3d& x1, const Vec3d& xnew1, size_t index1,
273
 
                                           const Vec3d& x2, const Vec3d& xnew2, size_t index2,
274
 
                                           const Vec3d& x3, const Vec3d& xnew3, size_t index3,
275
 
                                           double& bary1, double& bary2, double& bary3,
276
 
                                           Vec3d& normal,
277
 
                                           double& t,
278
 
                                           double& relative_normal_displacement,
279
 
                                           bool /*verbose*/ )
280
 
    {
281
 
        
282
 
        assert( index1 < index2 && index2 < index3 );
283
 
        
284
 
        const int segment_tetrahedron_test = 2;
285
 
        
286
 
        double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
287
 
        double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
288
 
        double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
289
 
        double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
290
 
        
291
 
        double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
292
 
        double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };  
293
 
        double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
294
 
        double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
295
 
        
296
 
        unsigned int num_intersections = 0;
297
 
        t = 2.0;
298
 
        double bary[6];
299
 
        bool any_degen = false;
300
 
        
301
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
302
 
                                    p0, pnew0, p1, p2, p3, pnew3,
303
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
304
 
        {
305
 
            ++num_intersections;
306
 
            
307
 
            bary1=0;
308
 
            bary2=0;
309
 
            bary3=1;      
310
 
            t=bary[1];
311
 
            normal=get_normal(x2-x1, x3-x1);
312
 
            relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew3-x3));
313
 
            
314
 
            for ( unsigned int i = 0; i < 6; ++i ) { if ( bary[i] == 0.0 ) { any_degen = true; }  }
315
 
        }
316
 
        
317
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
318
 
                                    p0, pnew0, p1, p2, pnew2, pnew3,
319
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
320
 
        {
321
 
            ++num_intersections;
322
 
            
323
 
            if( bary[1]<t )
324
 
            {
325
 
                bary1=0;
326
 
                bary2=(bary[4]+1e-300)/(bary[4]+bary[5]+2e-300); // guard against zero/zero
327
 
                bary3=1-bary2;                  
328
 
                t=bary[1];
329
 
                normal=get_normal(x2-x1, xnew3-xnew1);
330
 
                relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew2-x2));
331
 
            }
332
 
            
333
 
            for ( unsigned int i = 0; i < 6; ++i ) { if ( bary[i] == 0.0 ) { any_degen = true; }  }
334
 
        }
335
 
        
336
 
        if ( simplex_intersection4d( segment_tetrahedron_test,
337
 
                                    p0, pnew0, p1, pnew1, pnew2, pnew3,
338
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
339
 
        {
340
 
            ++num_intersections;
341
 
            
342
 
            if( bary[1]<t)
343
 
            {
344
 
                bary1=(bary[3]+1e-300)/(bary[3]+bary[4]+bary[5]+3e-300); // guard against zero/zero
345
 
                bary2=(bary[4]+1e-300)/(bary[3]+bary[4]+bary[5]+3e-300); // guard against zero/zero
346
 
                bary3=1-bary1-bary2;         
347
 
                t=bary[1];
348
 
                normal=get_normal(xnew2-xnew1, xnew3-xnew1);
349
 
                relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew1-x1));
350
 
            }
351
 
            
352
 
            for ( unsigned int i = 0; i < 6; ++i ) { if ( bary[i] == 0.0 ) { any_degen = true; }  }
353
 
            
354
 
        }
355
 
        
356
 
        if ( tunicate_verbose )
357
 
        {
358
 
            std::cout << "point-triangle, num_intersections: " << num_intersections << std::endl;
359
 
        }
360
 
        
361
 
        if ( num_intersections == 0 || num_intersections == 2 )
362
 
        {
363
 
            if ( any_degen ) 
364
 
            { 
365
 
                //g_stats.add_to_int( "tunicate_pt_degens", 1 );
366
 
                return true; 
367
 
            }
368
 
            
369
 
            return false;
370
 
        }
371
 
        
372
 
        return true;
373
 
        
374
 
    }
375
 
    
376
 
    // --------------------------------------------------------------------------------------------------------------
377
 
    
378
 
    bool tunicate_segment_segment_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
379
 
                                            const Vec3d& x1, const Vec3d& xnew1, size_t index1,
380
 
                                            const Vec3d& x2, const Vec3d& xnew2, size_t index2,
381
 
                                            const Vec3d& x3, const Vec3d& xnew3, size_t index3)
382
 
    {
383
 
        
384
 
        assert( index0 < index1 );
385
 
        assert( index2 < index3 );
386
 
        
387
 
        const int triangle_triangle_test = 3;
388
 
        
389
 
        double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
390
 
        double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
391
 
        double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
392
 
        double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
393
 
        double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
394
 
        double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
395
 
        double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
396
 
        double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
397
 
        
398
 
        unsigned int num_intersections = 0;
399
 
        double bary[6];
400
 
        
401
 
        if ( simplex_intersection4d( triangle_triangle_test,
402
 
                                    p0, p1, pnew1, p2, p3, pnew3,
403
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
404
 
        {
405
 
            ++num_intersections;
406
 
        }
407
 
        
408
 
        if ( simplex_intersection4d( triangle_triangle_test,
409
 
                                    p0, pnew0, pnew1, p2, p3, pnew3,
410
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
411
 
        {
412
 
            ++num_intersections;
413
 
        }
414
 
        
415
 
        if ( simplex_intersection4d( triangle_triangle_test,
416
 
                                    p0, p1, pnew1, p2, pnew2, pnew3,
417
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
418
 
        {
419
 
            ++num_intersections;
420
 
        }
421
 
        
422
 
        if ( simplex_intersection4d( triangle_triangle_test,
423
 
                                    p0, pnew0, pnew1, p2, pnew2, pnew3,
424
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
425
 
        {
426
 
            ++num_intersections;
427
 
        }
428
 
        
429
 
        if ( num_intersections % 2 == 0 )
430
 
        {
431
 
            return false;
432
 
        }
433
 
        
434
 
        return true;
435
 
        
436
 
    }
437
 
    
438
 
    
439
 
    // --------------------------------------------------------------------------------------------------------------
440
 
    
441
 
    bool tunicate_segment_segment_collision( const Vec3d& x0, const Vec3d& xnew0, size_t index0,
442
 
                                            const Vec3d& x1, const Vec3d& xnew1,  size_t index1,
443
 
                                            const Vec3d& x2, const Vec3d& xnew2,  size_t index2,
444
 
                                            const Vec3d& x3, const Vec3d& xnew3,  size_t index3,
445
 
                                            double& bary0, double& bary2,
446
 
                                            Vec3d& normal,
447
 
                                            double& t,
448
 
                                            double& relative_normal_displacement,
449
 
                                            bool /*verbose*/ )
450
 
    {
451
 
        
452
 
        assert( index0 < index1 );
453
 
        assert( index2 < index3 );
454
 
        
455
 
        const int triangle_triangle_test = 3;
456
 
        
457
 
        double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
458
 
        double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
459
 
        double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
460
 
        double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
461
 
        
462
 
        double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
463
 
        double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
464
 
        double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
465
 
        double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
466
 
        
467
 
        double bary[6];
468
 
        t = 2.0;
469
 
        
470
 
        unsigned int num_intersections = 0;
471
 
        
472
 
        std::vector<unsigned int> degen_counts(10, 0);
473
 
        
474
 
        if ( simplex_intersection4d( triangle_triangle_test,
475
 
                                    p0, p1, pnew1, p2, p3, pnew3,
476
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
477
 
        {
478
 
            ++num_intersections;
479
 
            
480
 
            if ( bary[0] == 0 ) { degen_counts[2]++; degen_counts[4]++; }
481
 
            if ( bary[1] == 0 ) { degen_counts[1]++; }
482
 
            if ( bary[2] == 0 ) { degen_counts[0]++; degen_counts[3]; }
483
 
            if ( bary[3] == 0 ) { degen_counts[7]++; degen_counts[9]++; }
484
 
            if ( bary[4] == 0 ) { degen_counts[6]++; }
485
 
            if ( bary[5] == 0 ) { degen_counts[5]++; degen_counts[8]++; }      
486
 
            
487
 
            if ( tunicate_verbose )
488
 
            {
489
 
                std::cout << "intersection A, barys: ";
490
 
                std::cout << bary[0] << " " << bary[1] << " " << bary[2] << " " << bary[3] << " " << bary[4] << " " << bary[5] << std::endl;
491
 
            }
492
 
            
493
 
            bary0=0;
494
 
            bary2=0;
495
 
            t=bary[2];
496
 
            normal=get_normal(x1-x0, x3-x2);
497
 
            relative_normal_displacement=dot(normal, (xnew1-x1)-(xnew3-x3));            
498
 
        }
499
 
        
500
 
        if ( simplex_intersection4d( triangle_triangle_test,
501
 
                                    p0, pnew0, pnew1, p2, p3, pnew3,
502
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
503
 
        {
504
 
            ++num_intersections;
505
 
            
506
 
            if ( bary[0] == 0 ) { degen_counts[2]++; degen_counts[4]++; }
507
 
            if ( bary[1] == 0 ) { degen_counts[1]++; }
508
 
            if ( bary[2] == 0 ) { degen_counts[0]++; degen_counts[3]; }
509
 
            if ( bary[3] == 0 ) { degen_counts[7]++; degen_counts[9]++; }
510
 
            if ( bary[4] == 0 ) { degen_counts[6]++; }
511
 
            if ( bary[5] == 0 ) { degen_counts[5]++; degen_counts[8]++; }
512
 
            
513
 
            if ( tunicate_verbose )
514
 
            {
515
 
                std::cout << "intersection B, barys: ";
516
 
                std::cout << bary[0] << " " << bary[1] << " " << bary[2] << " " << bary[3] << " " << bary[4] << " " << bary[5] << std::endl;
517
 
            }
518
 
            
519
 
            if( bary[5]<t )
520
 
            {
521
 
                bary0=(bary[1]+1e-300)/(bary[1]+bary[2]+2e-300); // guard against zero/zero
522
 
                bary2=0;
523
 
                t=bary[5];
524
 
                normal=get_normal(xnew1-xnew0, x3-x2);
525
 
                relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew3-x3));
526
 
            }
527
 
        }
528
 
        
529
 
        
530
 
        if ( simplex_intersection4d( triangle_triangle_test,
531
 
                                    p0, p1, pnew1, p2, pnew2, pnew3,
532
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
533
 
        {
534
 
            ++num_intersections;
535
 
            
536
 
            if ( bary[0] == 0 ) { degen_counts[2]++; degen_counts[4]++; }
537
 
            if ( bary[1] == 0 ) { degen_counts[1]++; }
538
 
            if ( bary[2] == 0 ) { degen_counts[0]++; degen_counts[3]; }
539
 
            if ( bary[3] == 0 ) { degen_counts[7]++; degen_counts[9]++; }
540
 
            if ( bary[4] == 0 ) { degen_counts[6]++; }
541
 
            if ( bary[5] == 0 ) { degen_counts[5]++; degen_counts[8]++; }
542
 
            
543
 
            if ( tunicate_verbose )
544
 
            {
545
 
                std::cout << "intersection C, barys: ";
546
 
                std::cout << bary[0] << " " << bary[1] << " " << bary[2] << " " << bary[3] << " " << bary[4] << " " << bary[5] << std::endl;
547
 
            }
548
 
            
549
 
            if( bary[2]<t )
550
 
            {
551
 
                bary0=0;
552
 
                bary2=(bary[4]+1e-300)/(bary[4]+bary[5]+2e-300); // guard against zero/zero
553
 
                t=bary[2];
554
 
                normal=get_normal(x1-x0, xnew3-xnew2);
555
 
                relative_normal_displacement=dot(normal, (xnew1-x1)-(xnew2-x2));
556
 
            }
557
 
        }
558
 
        
559
 
        if ( simplex_intersection4d( triangle_triangle_test,
560
 
                                    p0, pnew0, pnew1, p2, pnew2, pnew3,
561
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
562
 
        {
563
 
            
564
 
            ++num_intersections;
565
 
            
566
 
            if ( bary[0] == 0 ) { degen_counts[2]++; degen_counts[4]++; }
567
 
            if ( bary[1] == 0 ) { degen_counts[1]++; }
568
 
            if ( bary[2] == 0 ) { degen_counts[0]++; degen_counts[3]; }
569
 
            if ( bary[3] == 0 ) { degen_counts[7]++; degen_counts[9]++; }
570
 
            if ( bary[4] == 0 ) { degen_counts[6]++; }
571
 
            if ( bary[5] == 0 ) { degen_counts[5]++; degen_counts[8]++; }
572
 
            
573
 
            if ( tunicate_verbose )
574
 
            {
575
 
                std::cout << "intersection D, barys: ";
576
 
                std::cout << bary[0] << " " << bary[1] << " " << bary[2] << " " << bary[3] << " " << bary[4] << " " << bary[5] << std::endl;
577
 
            }
578
 
            
579
 
            if( 1-bary[0]<t)
580
 
            {
581
 
                bary0=(bary[1]+1e-300)/(bary[1]+bary[2]+2e-300); // guard against zero/zero
582
 
                bary2=(bary[4]+1e-300)/(bary[4]+bary[5]+2e-300); // guard against zero/zero
583
 
                t=1-bary[0];
584
 
                normal=get_normal(xnew1-xnew0, xnew3-xnew2);
585
 
                relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew2-x2));
586
 
            }
587
 
        }
588
 
        
589
 
        if ( tunicate_verbose )
590
 
        {
591
 
            std::cout << "edge-edge, num_intersections: " << num_intersections;
592
 
            std::cout << "degen_counts: ";
593
 
            for ( size_t i = 0; i < degen_counts.size(); ++i ) { std::cout << degen_counts[i] << " "; }
594
 
            std::cout << std::endl;
595
 
            
596
 
        }
597
 
        
598
 
        if ( num_intersections % 2 == 0 )
599
 
        {
600
 
            for ( size_t i = 0; i < degen_counts.size(); ++i ) 
601
 
            { 
602
 
                if( degen_counts[i] > 0 ) 
603
 
                { 
604
 
                    //g_stats.add_to_int( "tunicate_ee_degens", 1 );
605
 
                    return true; 
606
 
                } 
607
 
            }
608
 
            
609
 
            return false;
610
 
        }
611
 
        
612
 
        return true;
613
 
        
614
 
    }
615
 
    
616
 
}     // namespace
617
 
 
618
 
 
619
 
// --------------------------------------------------------------------------------------------------
620
 
// 2D Continuous collision detection
621
 
// --------------------------------------------------------------------------------------------------
622
 
 
623
 
// --------------------------------------------------------------------------------------------------------------
624
 
 
625
 
bool point_segment_collision( const Vec2d& x0, const Vec2d& xnew0, size_t index0,
626
 
                             const Vec2d& x1, const Vec2d& xnew1, size_t index1,
627
 
                             const Vec2d& x2, const Vec2d& xnew2, size_t index2 )
628
 
{
629
 
    return tunicate_point_segment_collision( x0, xnew0, index0, 
630
 
                                            x1, xnew1, index1, 
631
 
                                            x2, xnew2, index2 );
632
 
}
633
 
 
634
 
bool point_segment_collision(const Vec2d& x0, const Vec2d& xnew0, size_t index0,
635
 
                             const Vec2d& x1, const Vec2d& xnew1, size_t index1,
636
 
                             const Vec2d& x2, const Vec2d& xnew2, size_t index2,
637
 
                             double& edge_alpha, Vec2d& normal, double& time, double& relative_normal_displacement )
638
 
{
639
 
    bool tunicate_result = tunicate_point_segment_collision( x0, xnew0, index0, 
640
 
                                                            x1, xnew1, index1, 
641
 
                                                            x2, xnew2, index2,
642
 
                                                            edge_alpha, normal, time, relative_normal_displacement );
643
 
    
644
 
    return tunicate_result;
645
 
    
646
 
}
647
 
 
648
 
 
649
 
// --------------------------------------------------------------------------------------------------
650
 
// 2D Static intersection detection / distance queries
651
 
// --------------------------------------------------------------------------------------------------
652
 
 
653
 
// --------------------------------------------------------------------------------------------------------------
654
 
 
655
 
bool segment_segment_intersection(const Vec2d& x0, size_t /*index0*/, 
656
 
                                  const Vec2d& x1, size_t /*index1*/,
657
 
                                  const Vec2d& x2, size_t /*index2*/,
658
 
                                  const Vec2d& x3, size_t /*index3*/)
659
 
{
660
 
    double bary[4];   // not returned   
661
 
    return simplex_intersection2d( 2, x0.v, x1.v, x2.v, x3.v, &bary[0], &bary[1], &bary[2], &bary[3] );
662
 
}
663
 
 
664
 
// --------------------------------------------------------------------------------------------------------------
665
 
 
666
 
bool segment_segment_intersection(const Vec2d& x0, size_t /*index0*/, 
667
 
                                  const Vec2d& x1, size_t /*index1*/,
668
 
                                  const Vec2d& x2, size_t /*index2*/,
669
 
                                  const Vec2d& x3, size_t /*index3*/,
670
 
                                  double &s0, double& s2 )
671
 
{
672
 
    double s1, s3;    // not returned
673
 
    return simplex_intersection2d( 2, x0.v, x1.v, x2.v, x3.v, &s0, &s1, &s2, &s3 );
674
 
}
675
 
 
676
 
 
677
 
// --------------------------------------------------------------------------------------------------
678
 
// 3D Continuous collision detection
679
 
// --------------------------------------------------------------------------------------------------
680
 
 
681
 
 
682
 
bool point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
683
 
                              const Vec3d& x1, const Vec3d& xnew1, size_t index1,
684
 
                              const Vec3d& x2, const Vec3d& xnew2, size_t index2,
685
 
                              const Vec3d& x3, const Vec3d& xnew3, size_t index3 )
686
 
{
687
 
    bool tunicate_result = tunicate_point_triangle_collision( x0, xnew0, index0,
688
 
                                                             x1, xnew1, index1,
689
 
                                                             x2, xnew2, index2,
690
 
                                                             x3, xnew3, index3 );                                            
691
 
    
692
 
    return tunicate_result;
693
 
    
694
 
} 
695
 
 
696
 
// --------------------------------------------------------------------------------------------------------------
697
 
 
698
 
bool point_triangle_collision( const Vec3d& x0, const Vec3d& xnew0, size_t index0,
699
 
                              const Vec3d& x1, const Vec3d& xnew1, size_t index1,
700
 
                              const Vec3d& x2, const Vec3d& xnew2, size_t index2,
701
 
                              const Vec3d& x3, const Vec3d& xnew3, size_t index3,
702
 
                              double& bary1, double& bary2, double& bary3,
703
 
                              Vec3d& normal,
704
 
                              double& relative_normal_displacement )
705
 
{
706
 
    
707
 
    double time;
708
 
    bool verbose = false;
709
 
    bool tunicate_result = tunicate_point_triangle_collision( x0, xnew0, index0,
710
 
                                                             x1, xnew1, index1,
711
 
                                                             x2, xnew2, index2,
712
 
                                                             x3, xnew3, index3,
713
 
                                                             bary1, bary2, bary3,
714
 
                                                             normal, time, relative_normal_displacement, verbose );
715
 
    
716
 
    return tunicate_result;
717
 
    
718
 
}
719
 
 
720
 
 
721
 
// --------------------------------------------------------------------------------------------------------------
722
 
 
723
 
 
724
 
bool segment_segment_collision(const Vec3d& x0, const Vec3d& xnew0, size_t index0,
725
 
                               const Vec3d& x1, const Vec3d& xnew1, size_t index1,
726
 
                               const Vec3d& x2, const Vec3d& xnew2, size_t index2,
727
 
                               const Vec3d& x3, const Vec3d& xnew3, size_t index3)
728
 
{
729
 
    bool tunicate_result = tunicate_segment_segment_collision( x0, xnew0, index0,
730
 
                                                              x1, xnew1, index1,
731
 
                                                              x2, xnew2, index2,
732
 
                                                              x3, xnew3, index3 );
733
 
    
734
 
    return tunicate_result;
735
 
}
736
 
 
737
 
// --------------------------------------------------------------------------------------------------------------
738
 
 
739
 
 
740
 
bool segment_segment_collision( const Vec3d& x0, const Vec3d& xnew0, size_t index0,
741
 
                               const Vec3d& x1, const Vec3d& xnew1, size_t index1,
742
 
                               const Vec3d& x2, const Vec3d& xnew2, size_t index2,
743
 
                               const Vec3d& x3, const Vec3d& xnew3, size_t index3,
744
 
                               double& bary0, double& bary2,
745
 
                               Vec3d& normal,
746
 
                               double& relative_normal_displacement )
747
 
{
748
 
    double time;
749
 
    bool verbose = false;
750
 
    bool tunicate_result = tunicate_segment_segment_collision( x0, xnew0, index0,
751
 
                                                              x1, xnew1, index1,
752
 
                                                              x2, xnew2, index2,
753
 
                                                              x3, xnew3, index3,
754
 
                                                              bary0, bary2, normal, time, relative_normal_displacement, verbose );
755
 
    
756
 
    return tunicate_result;
757
 
    
758
 
}
759
 
 
760
 
 
761
 
// --------------------------------------------------------------------------------------------------
762
 
// 3D Static intersection detection
763
 
// --------------------------------------------------------------------------------------------------
764
 
 
765
 
// --------------------------------------------------------------------------------------------------
766
 
 
767
 
// x0-x1 is the segment and and x2-x3-x4 is the triangle.
768
 
bool segment_triangle_intersection(const Vec3d& x0, size_t /*index0*/,
769
 
                                   const Vec3d& x1, size_t /*index1*/,
770
 
                                   const Vec3d& x2, size_t /*index2*/,
771
 
                                   const Vec3d& x3, size_t /*index3*/,
772
 
                                   const Vec3d& x4, size_t /*index4*/,
773
 
                                   bool /*degenerate_counts_as_intersection*/,
774
 
                                   bool /*verbose*/ )
775
 
{
776
 
    double bary[5];
777
 
    return simplex_intersection3d( 2, x0.v, x1.v, x2.v, x3.v, x4.v, &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] );
778
 
}
779
 
 
780
 
 
781
 
bool segment_triangle_intersection(const Vec3d& x0, size_t /*index0*/,
782
 
                                   const Vec3d& x1, size_t /*index1*/,
783
 
                                   const Vec3d& x2, size_t /*index2*/,
784
 
                                   const Vec3d& x3, size_t /*index3*/,
785
 
                                   const Vec3d& x4, size_t /*index4*/,
786
 
                                   double& bary0, double& bary1, double& bary2, double& bary3, double& bary4,
787
 
                                   bool /*degenerate_counts_as_intersection*/,
788
 
                                   bool /*verbose*/ )
789
 
{
790
 
    return simplex_intersection3d( 2, x0.v, x1.v, x2.v, x3.v, x4.v, &bary0, &bary1, &bary2, &bary3, &bary4 );
791
 
}
792
 
 
793
 
 
794
 
// --------------------------------------------------------------------------------------------------
795
 
 
796
 
 
797
 
// x0 is the point and x1-x2-x3-x4 is the tetrahedron. Order is irrelevant.
798
 
bool point_tetrahedron_intersection(const Vec3d& x0, size_t /*index0*/,
799
 
                                    const Vec3d& x1, size_t /*index1*/,
800
 
                                    const Vec3d& x2, size_t /*index2*/,
801
 
                                    const Vec3d& x3, size_t /*index3*/,
802
 
                                    const Vec3d& x4, size_t /*index4*/)
803
 
{
804
 
    double bary[5];
805
 
    return simplex_intersection3d( 1, x0.v, x1.v, x2.v, x3.v, x4.v, &bary[0], &bary[1], &bary[2], &bary[3], &bary[4] );   
806
 
}
807
 
 
808
 
 
809
 
#endif
810
 
 
811