← Back to branch summary

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

« back to all changes in this revision

Viewing changes to extern/eltopo/common/sos_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/sos_ccd_wrapper.cpp
1
 
 
2
 
// ---------------------------------------------------------
3
 
//
4
 
//  sos_ccd_wrapper.cpp
5
 
//  Tyson Brochu 2009
6
 
//
7
 
//  Tunicate-based implementation of collision and intersection queries.  Uses exact arithmetic with SoS.
8
 
//  (See Robert Bridson's "Tunicate" library.)
9
 
//
10
 
// ---------------------------------------------------------
11
 
 
12
 
#include <ccd_wrapper.h>
13
 
 
14
 
#ifdef USE_SOS_TUNICATE_CCD
15
 
 
16
 
 
17
 
#include <tunicate.h>
18
 
#include <vec.h>
19
 
 
20
 
const unsigned int nv = 1000000;
21
 
 
22
 
// return a unit-length vector orthogonal to u and v
23
 
static Vec3d get_normal(const Vec3d& u, const Vec3d& v)
24
 
{
25
 
   Vec3d c=cross(u,v);
26
 
   double m=mag(c);
27
 
   if(m) return c/m;
28
 
   // degenerate case: either u and v are parallel, or at least one is zero; pick an arbitrary orthogonal vector
29
 
   if(mag2(u)>=mag2(v)){
30
 
      if(std::fabs(u[0])>=std::fabs(u[1]) && std::fabs(u[0])>=std::fabs(u[2]))
31
 
         c=Vec3d(-u[1]-u[2], u[0], u[0]);
32
 
      else if(std::fabs(u[1])>=std::fabs(u[2]))
33
 
         c=Vec3d(u[1], -u[0]-u[2], u[1]);
34
 
      else
35
 
         c=Vec3d(u[2], u[2], -u[0]-u[1]);
36
 
   }else{
37
 
      if(std::fabs(v[0])>=std::fabs(v[1]) && std::fabs(v[0])>=std::fabs(v[2]))
38
 
         c=Vec3d(-v[1]-v[2], v[0], v[0]);
39
 
      else if(std::fabs(v[1])>=std::fabs(v[2]))
40
 
         c=Vec3d(v[1], -v[0]-v[2], v[1]);
41
 
      else
42
 
         c=Vec3d(v[2], v[2], -v[0]-v[1]);
43
 
   }
44
 
   m=mag(c);
45
 
   if(m) return c/m;
46
 
   // really degenerate case: u and v are both zero vectors; pick a random unit-length vector
47
 
   c[0]=random()%2 ? -0.577350269189626 : 0.577350269189626;
48
 
   c[1]=random()%2 ? -0.577350269189626 : 0.577350269189626;
49
 
   c[2]=random()%2 ? -0.577350269189626 : 0.577350269189626;
50
 
   return c;
51
 
}
52
 
 
53
 
// --------------------------------------------------------------------------------------------------------------
54
 
 
55
 
 
56
 
bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, unsigned int index0,
57
 
                                       const Vec3d& x1, const Vec3d& xnew1, unsigned int index1,
58
 
                                       const Vec3d& x2, const Vec3d& xnew2, unsigned int index2,
59
 
                                       const Vec3d& x3, const Vec3d& xnew3, unsigned int index3 )
60
 
{
61
 
   
62
 
   const int segment_tetrahedron_test = 2;
63
 
 
64
 
   double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
65
 
   double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
66
 
   double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
67
 
   double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
68
 
   double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
69
 
   double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
70
 
   double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
71
 
   double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
72
 
   
73
 
   double a0, a1, a2, a3, a4, a5;
74
 
   
75
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
76
 
                                    index0, p0,
77
 
                                    index0 + nv, pnew0,
78
 
                                    index1, p1,
79
 
                                    index2, p2,
80
 
                                    index3, p3,
81
 
                                    index3 + nv, pnew3,
82
 
                                    &a0, &a1, &a2, &a3, &a4, &a5 ) )
83
 
   {
84
 
      return true;
85
 
   }
86
 
 
87
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
88
 
                                    index0, p0,
89
 
                                    index0 + nv, pnew0,
90
 
                                    index1, p1,
91
 
                                    index2, p2,
92
 
                                    index2 + nv, pnew2,
93
 
                                    index3 + nv, pnew3,
94
 
                                    &a0, &a1, &a2, &a3, &a4, &a5 ) )
95
 
   {
96
 
      return true;
97
 
   }
98
 
 
99
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
100
 
                                    index0, p0,
101
 
                                    index0 + nv, pnew0,
102
 
                                    index1, p1,
103
 
                                    index1 + nv, pnew1,
104
 
                                    index2 + nv, pnew2,
105
 
                                    index3 + nv, pnew3,
106
 
                                    &a0, &a1, &a2, &a3, &a4, &a5 ) )
107
 
   {
108
 
      return true;
109
 
   }
110
 
   
111
 
   return false;
112
 
}
113
 
 
114
 
 
115
 
// --------------------------------------------------------------------------------------------------------------
116
 
 
117
 
 
118
 
bool tunicate_point_triangle_collision(const Vec3d& x0, const Vec3d& xnew0, unsigned int index0,
119
 
                                       const Vec3d& x1, const Vec3d& xnew1, unsigned int index1,
120
 
                                       const Vec3d& x2, const Vec3d& xnew2, unsigned int index2,
121
 
                                       const Vec3d& x3, const Vec3d& xnew3, unsigned int index3,
122
 
                                       double& bary1, double& bary2, double& bary3,
123
 
                                       Vec3d& normal,
124
 
                                       double& t,
125
 
                                       double& relative_normal_displacement,
126
 
                                       bool verbose )
127
 
{
128
 
 
129
 
   const int segment_tetrahedron_test = 2;
130
 
   
131
 
   double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
132
 
   double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
133
 
   double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
134
 
   double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
135
 
   double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
136
 
   double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
137
 
   double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
138
 
   double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
139
 
   
140
 
   bool collision=false;
141
 
   double bary[6];
142
 
   
143
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
144
 
                                    index0, p0,
145
 
                                    index0 + nv, pnew0,
146
 
                                    index1, p1,
147
 
                                    index2, p2,
148
 
                                    index3, p3,
149
 
                                    index3 + nv, pnew3,
150
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
151
 
   {
152
 
      collision=true;
153
 
      t=bary[1];
154
 
      normal=get_normal(x2-x1, x3-x1);
155
 
      relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew3-x3));
156
 
   }
157
 
   
158
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
159
 
                                   index0, p0,
160
 
                                   index0 + nv, pnew0,
161
 
                                   index1, p1,
162
 
                                   index2, p2,
163
 
                                   index2 + nv, pnew2,
164
 
                                   index3 + nv, pnew3,
165
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
166
 
   {
167
 
      if(!collision || bary[1]<t)
168
 
      {
169
 
         collision=true;
170
 
         t=bary[1];
171
 
         normal=get_normal(x2-x1, xnew3-xnew2);
172
 
         relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew2-x2));
173
 
      }
174
 
   }
175
 
 
176
 
   if ( sos_simplex_intersection4d( segment_tetrahedron_test,
177
 
                                   index0, p0,
178
 
                                   index0 + nv, pnew0,
179
 
                                   index1, p1,
180
 
                                   index1 + nv, pnew1,
181
 
                                   index2 + nv, pnew2,
182
 
                                   index3 + nv, pnew3,
183
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
184
 
   {
185
 
      if(!collision || bary[1]<t)
186
 
      {
187
 
         collision=true;
188
 
         t=bary[1];
189
 
         normal=get_normal(xnew2-xnew1, xnew3-xnew1);
190
 
         relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew1-x1));
191
 
      }
192
 
   }
193
 
   
194
 
   return collision;
195
 
}
196
 
 
197
 
 
198
 
// --------------------------------------------------------------------------------------------------------------
199
 
 
200
 
 
201
 
bool tunicate_segment_segment_collision(const Vec3d& x0, const Vec3d& xnew0, unsigned int index0,
202
 
                                        const Vec3d& x1, const Vec3d& xnew1, unsigned int index1,
203
 
                                        const Vec3d& x2, const Vec3d& xnew2, unsigned int index2,
204
 
                                        const Vec3d& x3, const Vec3d& xnew3, unsigned int index3)
205
 
{
206
 
   
207
 
   const int triangle_triangle_test = 3;
208
 
   
209
 
   double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
210
 
   double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
211
 
   double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
212
 
   double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
213
 
   double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
214
 
   double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
215
 
   double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
216
 
   double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
217
 
   
218
 
   double bary[6];
219
 
   
220
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
221
 
                                    index0, p0,
222
 
                                    index1, p1,
223
 
                                    index1, p1,
224
 
                                    index2, p2,
225
 
                                    index3, p3,
226
 
                                    index3 + nv, pnew3,
227
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
228
 
   {
229
 
      return true;
230
 
   }
231
 
 
232
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
233
 
                                    index0, p0,
234
 
                                    index0 + nv, pnew0,
235
 
                                    index1 + nv, pnew1,
236
 
                                    index2, p2,
237
 
                                    index3, p3,
238
 
                                    index3 + nv, pnew3,
239
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
240
 
   {
241
 
      return true;
242
 
   }
243
 
   
244
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
245
 
                                    index0, p0,
246
 
                                    index1, p1,
247
 
                                    index1 + nv, pnew1,
248
 
                                    index2, p2,
249
 
                                    index2 + nv, pnew2,
250
 
                                    index3 + nv, pnew3,
251
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
252
 
   {
253
 
      return true;
254
 
   }
255
 
 
256
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
257
 
                                    index0, p0,
258
 
                                    index0 + nv, pnew0,
259
 
                                    index1 + nv, pnew1,
260
 
                                    index2, p2,
261
 
                                    index2 + nv, pnew2,
262
 
                                    index3 + nv, pnew3,
263
 
                                    &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
264
 
   {
265
 
      return true;
266
 
   }
267
 
 
268
 
   return false;
269
 
}
270
 
 
271
 
 
272
 
// --------------------------------------------------------------------------------------------------------------
273
 
 
274
 
 
275
 
 
276
 
bool tunicate_segment_segment_collision(const Vec3d& x0, const Vec3d& xnew0, unsigned int index0,
277
 
                                        const Vec3d& x1, const Vec3d& xnew1, unsigned int index1,
278
 
                                        const Vec3d& x2, const Vec3d& xnew2, unsigned int index2,
279
 
                                        const Vec3d& x3, const Vec3d& xnew3, unsigned int index3,
280
 
                                        double& bary0, double& bary2,
281
 
                                        Vec3d& normal,
282
 
                                        double& t,
283
 
                                        double& relative_normal_displacement,
284
 
                                        bool verbose )
285
 
{
286
 
 
287
 
   const int triangle_triangle_test = 3;
288
 
   
289
 
   double p0[4] = { x0[0], x0[1], x0[2], 0.0 };
290
 
   double pnew0[4] = { xnew0[0], xnew0[1], xnew0[2], 1.0 };
291
 
   double p1[4] = { x1[0], x1[1], x1[2], 0.0 };
292
 
   double pnew1[4] = { xnew1[0], xnew1[1], xnew1[2], 1.0 };
293
 
   double p2[4] = { x2[0], x2[1], x2[2], 0.0 };
294
 
   double pnew2[4] = { xnew2[0], xnew2[1], xnew2[2], 1.0 };
295
 
   double p3[4] = { x3[0], x3[1], x3[2], 0.0 };
296
 
   double pnew3[4] = { xnew3[0], xnew3[1], xnew3[2], 1.0 };
297
 
   
298
 
   bool collision=false;
299
 
   double bary[6];
300
 
   
301
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
302
 
                                   index0, p0,
303
 
                                   index1, p1,
304
 
                                   index1 + nv, pnew1,
305
 
                                   index2, p2,
306
 
                                   index3, p3,
307
 
                                   index3 + nv, pnew3,
308
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
309
 
   {
310
 
      collision=true;
311
 
      bary0=0;
312
 
      bary2=0;
313
 
      t=bary[2];
314
 
      normal=get_normal(x1-x0, x3-x2);
315
 
      relative_normal_displacement=dot(normal, (xnew1-x1)-(xnew3-x3));
316
 
   }
317
 
   
318
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
319
 
                                   index0, p0,
320
 
                                   index0 + nv, pnew0,
321
 
                                   index1 + nv, pnew1,
322
 
                                   index2, p2,
323
 
                                   index3, p3,
324
 
                                   index3 + nv, pnew3,
325
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
326
 
   {
327
 
      if(!collision || bary[5]<t)
328
 
      {
329
 
         collision=true;
330
 
         bary0=0.5;//(bary[1]+1e-300)/(bary[1]+bary[2]+2e-300); // guard against zero/zero
331
 
         bary2=0;
332
 
         t=bary[5];
333
 
         normal=get_normal(xnew1-xnew0, x3-x2);
334
 
         relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew3-x3));
335
 
      }
336
 
   }
337
 
   
338
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
339
 
                                   index0, p0,
340
 
                                   index1, p1,
341
 
                                   index1 + nv, pnew1,
342
 
                                   index2, p2,
343
 
                                   index2 + nv, pnew2,
344
 
                                   index3 + nv, pnew3,
345
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
346
 
   {
347
 
      if(!collision || bary[2]<t){
348
 
         collision=true;
349
 
         bary0=0;
350
 
         bary2=0.5;//(bary[4]+1e-300)/(bary[4]+bary[5]+2e-300); // guard against zero/zero
351
 
         t=bary[2];
352
 
         normal=get_normal(x1-x0, xnew3-xnew2);
353
 
         relative_normal_displacement=dot(normal, (xnew1-x1)-(xnew2-x2));
354
 
      }
355
 
   }
356
 
   
357
 
   if ( sos_simplex_intersection4d( triangle_triangle_test,
358
 
                                   index0, p0,
359
 
                                   index0 + nv, pnew0,
360
 
                                   index1 + nv, pnew1,
361
 
                                   index2, p2,
362
 
                                   index2 + nv, pnew2,
363
 
                                   index3 + nv, pnew3,
364
 
                                   &bary[0], &bary[1], &bary[2], &bary[3], &bary[4], &bary[5] ) )
365
 
   {
366
 
      if(!collision || 1-bary[0]<t){
367
 
         collision=true;
368
 
         bary0=0.5;//(bary[1]+1e-300)/(bary[1]+bary[2]+2e-300); // guard against zero/zero
369
 
         bary2=0.5;//(bary[4]+1e-300)/(bary[4]+bary[5]+2e-300); // guard against zero/zero
370
 
         t=1-bary[0];
371
 
         normal=get_normal(xnew1-xnew0, xnew3-xnew2);
372
 
         relative_normal_displacement=dot(normal, (xnew0-x0)-(xnew2-x2));
373
 
      }
374
 
   }
375
 
 
376
 
   return collision;
377
 
   
378
 
}
379
 
 
380
 
 
381
 
 
382
 
#endif
383
 
 
384