← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Collision/TriOverlap.cpp

  • Committer: Thomas Moulard
  • Date: 2012-10-23 12:43:24 UTC
  • Revision ID: git-v1:351cf736ad49bc7a9a7b9767dee760a013517a5d
Tags: upstream/1.1.0
Imported Upstream version 1.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Collision/TriOverlap.cpp
 
1
//
 
2
// TriOverlap.cpp
 
3
//
 
4
 
 
5
#include "CollisionPairInserter.h"
 
6
#include <cmath>
 
7
#include <cstdio>
 
8
#include <iostream>
 
9
 
 
10
using namespace std;
 
11
using namespace cnoid;
 
12
 
 
13
namespace {
 
14
 
 
15
    const bool HIRUKAWA_DEBUG = false;
 
16
 
 
17
    /* used in normal_test */
 
18
    enum { NOT_INTERSECT = 0,
 
19
           EDGE1_NOT_INTERSECT = 1,
 
20
           EDGE2_NOT_INTERSECT = 2,
 
21
           EDGE3_NOT_INTERSECT = 3 };
 
22
 
 
23
    /* used in cross_test */
 
24
    const int INTERSECT = 1;
 
25
}
 
26
 
 
27
 
 
28
/**********************************************************     
 
29
  separability test by the supporting plane of a triangle
 
30
   return value 
 
31
   0   : not intersect
 
32
   1   : f1 or e1 doesn't intersect
 
33
   2   : f2 or e2 doesn't intersect
 
34
   3   : f3 or e3 doesn't intersect
 
35
**********************************************************/     
 
36
static int separability_test_by_face(const Vector3& nm)
 
37
{
 
38
    if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] < 0.0 ||
 
39
       nm[0] > 0.0 && nm[1] > 0.0 && nm[2] > 0.0){
 
40
        return NOT_INTERSECT;
 
41
    }
 
42
    if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] > 0.0 ||
 
43
       nm[0] > 0.0 && nm[1] > 0.0 && nm[2] < 0.0){
 
44
        return EDGE1_NOT_INTERSECT;
 
45
    }
 
46
    if(nm[0] < 0.0 && nm[1] > 0.0 && nm[2] > 0.0 ||
 
47
       nm[0] > 0.0 && nm[1] < 0.0 && nm[2] < 0.0){
 
48
        return EDGE2_NOT_INTERSECT;
 
49
    }
 
50
    if(nm[0] > 0.0 && nm[1] < 0.0 && nm[2] > 0.0 ||
 
51
       nm[0] < 0.0 && nm[1] > 0.0 && nm[2] < 0.0){
 
52
        return EDGE3_NOT_INTERSECT;
 
53
    }
 
54
    return 0;
 
55
}
 
56
 
 
57
 
 
58
/**********************************************************
 
59
   triangle inside test:
 
60
   normal vector is cross product of ei*fj
 
61
***********************************************************/
 
62
static int triangle_inside_test(
 
63
    const Vector3& ef1,
 
64
    const Vector3& ef2,
 
65
    const Vector3& ef3,
 
66
    const Vector3& P3,
 
67
    const Vector3& P1,
 
68
    const Vector3& P2,
 
69
    const Vector3& Q)
 
70
{       
 
71
    double ef1P1 = ef1.dot(P1); /*project P1 on ef1*/
 
72
    double ef1P3 = ef1.dot(P3); /*project P3 on ef1*/
 
73
    double ef1Q  = ef1.dot(Q);  /*project Q on ef1*/
 
74
 
 
75
    double ef2P2 = ef2.dot(P2); /*project P2 on ef2*/
 
76
    double ef2P1 = ef2.dot(P1); /*project P1 on ef2*/   
 
77
    double ef2Q  = ef2.dot(Q);   /*project Q on ef2*/
 
78
 
 
79
    double ef3P3 = ef3.dot(P3);  /*project P3 on ef3*/  
 
80
    double ef3P2 = ef3.dot(P2);  /*project P2 on ef3*/          
 
81
    double ef3Q  = ef3.dot(Q);   /*project Q on ef3*/           
 
82
 
 
83
    if((ef1P3 > ef1P1 && ef1Q > ef1P1   ||
 
84
        ef1P3 < ef1P1 && ef1Q < ef1P1     ) 
 
85
       &&
 
86
       (ef2P1 > ef2P2 && ef2Q > ef2P2   ||
 
87
        ef2P1 < ef2P2 && ef2Q < ef2P2     ) 
 
88
       &&
 
89
       (ef3P2 > ef3P3 && ef3Q > ef3P3   ||
 
90
        ef3P2 < ef3P3 && ef3Q < ef3P3     )) {
 
91
        return INTERSECT;
 
92
    }
 
93
 
 
94
    return NOT_INTERSECT;
 
95
}
 
96
 
 
97
 
 
98
static void find_intersection_pt(
 
99
    Vector3& ipt,
 
100
    const Vector3& x1,
 
101
    const Vector3& x2,
 
102
    double mn1, 
 
103
    double mn2)
 
104
{
 
105
    if(mn1 == mn2) /*exit(0);*/ return;
 
106
 
 
107
    if(mn1 >0 && mn2 < 0){
 
108
        ipt = (-(mn2*x1) + mn1*x2)/(mn1-mn2);
 
109
    }else if(mn1 < 0 && mn2 > 0){
 
110
        ipt = (mn2*x1 - mn1*x2)/(-mn1+mn2);
 
111
    }
 
112
}
 
113
 
 
114
 
 
115
static inline void find_intersection_pt(
 
116
    Vector3& ipt,
 
117
    const Vector3& x1,
 
118
    const Vector3& x2,
 
119
    double p)
 
120
{
 
121
    ipt = (1.0 - p) * x1 + p * x2;
 
122
 
 
123
    if(HIRUKAWA_DEBUG){
 
124
        cout << "v1 = " << x1[0] << ", " << x1[1] << ", " << x1[2] << endl; 
 
125
        cout << "v2 = " << x2[0] << ", " << x2[1] << ", " << x2[2] << endl;
 
126
        cout << "edge = " << x2[0]-x1[0] << ", " << x2[1]-x1[1] << ", "
 
127
             << x2[2]-x1[2] << endl;
 
128
        cout << "common pt = " << ipt[0] << " " << ipt[1] << " " << ipt[2] << endl;
 
129
    }
 
130
}
 
131
 
 
132
 
 
133
//
 
134
// Calculate the depth of the intersection between two triangles
 
135
//
 
136
static inline double calc_depth(
 
137
    const Vector3& ip1,
 
138
    const Vector3& ip2,
 
139
    const Vector3& n)
 
140
{
 
141
    // vecNormalize(n);
 
142
 
 
143
    if(HIRUKAWA_DEBUG){
 
144
        cout << "calc_depth 1 = " << (ip1 - ip2).dot(n) << endl;
 
145
    }
 
146
    
 
147
    return fabs((ip1 - ip2).dot(n));
 
148
}
 
149
 
 
150
 
 
151
static double calc_depth(
 
152
    const Vector3& ip,
 
153
    const Vector3& pt1,
 
154
    const Vector3& pt2,
 
155
    const Vector3& n)
 
156
{
 
157
    double d1 = fabs((ip - pt1).dot(n));
 
158
    double d2 = fabs((ip - pt2).dot(n));
 
159
    double depth = (d1 < d2) ? d1 : d2;
 
160
 
 
161
    if(HIRUKAWA_DEBUG){
 
162
        cout << "calc_depth 2 = " << depth << endl;
 
163
    }
 
164
    
 
165
    return depth;
 
166
} 
 
167
 
 
168
 
 
169
static double calc_depth(
 
170
    const Vector3& ip1,
 
171
    const Vector3& ip2,
 
172
    const Vector3& pt1,
 
173
    const Vector3& pt2,
 
174
    const Vector3& pt3,
 
175
    const Vector3& n)
 
176
{
 
177
    // when a triangle penetrates another triangle at two intersection points
 
178
    // and the separating plane is the supporting plane of the penetrated triangle
 
179
    
 
180
    // vecNormalize(n);
 
181
    
 
182
    double d1 = fabs((ip1 - pt1).dot(n));
 
183
    double d2 = fabs((ip2 - pt2).dot(n));
 
184
    double d3 = fabs((ip1 - pt3).dot(n)); // ip1 can be either ip1 or ip2
 
185
    
 
186
    double depth = (d1 < d2) ? d2 : d1;
 
187
    if(d3 < depth){
 
188
        depth = d3;
 
189
    }
 
190
    
 
191
    if(HIRUKAWA_DEBUG){
 
192
        cout << "calc_depth 3 = " << depth << endl;
 
193
    }
 
194
    
 
195
    return depth;
 
196
}
 
197
 
 
198
 
 
199
static void find_foot(
 
200
    const Vector3& ip,
 
201
    const Vector3& pt1,
 
202
    const Vector3& pt2,
 
203
    Vector3& f)
 
204
{
 
205
    /*
 
206
    double u, v, w, p;
 
207
 
 
208
    u = pt2[0] - pt1[0]; v = pt2[1] - pt1[1]; w = pt2[2] - pt1[2];
 
209
    
 
210
    p = u * (ip[0] - pt1[0]) + v * (ip[1] - pt1[1]) + w * (ip[2] - pt1[2]);
 
211
    p /= u * u + v * v + w * w;
 
212
    
 
213
    f[0] = pt1[0] + u * p; f[1] = pt1[1] + v * p; f[2] = pt1[2] + w * p;
 
214
    */
 
215
 
 
216
    const Vector3 pt = pt2 - pt1;
 
217
    const double p = pt.dot(ip - pt1) / pt.dot(pt);
 
218
    f = pt1 + p * pt;
 
219
}
 
220
 
 
221
 
 
222
static double calc_depth(
 
223
    const Vector3& ip,
 
224
    const Vector3& pt1,
 
225
    const Vector3& pt2,
 
226
    const Vector3& pt3,
 
227
    const Vector3& n)
 
228
{
 
229
    Vector3 f12, f23, f31;
 
230
    
 
231
    find_foot(ip, pt1, pt2, f12);
 
232
    find_foot(ip, pt2, pt3, f23);
 
233
    find_foot(ip, pt3, pt1, f31);
 
234
    
 
235
    if(HIRUKAWA_DEBUG){
 
236
        cout << "ip = " << ip[0] << " " << ip[1] << " " << ip[2] << endl;
 
237
        cout << "f12 = " << f12[0] << " " << f12[1] << " " << f12[2] << endl;
 
238
        cout << "f23 = " << f23[0] << " " << f23[1] << " " << f23[2] << endl;
 
239
        cout << "f31 = " << f31[0] << " " << f31[1] << " " << f31[2] << endl;
 
240
    }
 
241
    
 
242
    // fabs() is taken to cope with numerical error of find_foot()
 
243
    const double d1 = fabs((f12 - ip).dot(n));
 
244
    const double d2 = fabs((f23 - ip).dot(n));
 
245
    const double d3 = fabs((f31 - ip).dot(n));
 
246
    
 
247
    // cout << "d1 d2 d3 = " << d1 << " " << d2 << " " << d3 << endl;
 
248
    // dsum = fabs(d1)+fabs(d2)+fabs(d3);
 
249
    // if(d1<0.0) d1=dsum; if(d2<0.0) d2=dsum; if(d3<0.0) d3=dsum;
 
250
    
 
251
    double depth = (d1 < d2) ? d1 : d2;
 
252
    if(d3 < depth){
 
253
        depth = d3;
 
254
    }
 
255
    
 
256
    if(HIRUKAWA_DEBUG){
 
257
        cout << "calc_depth 4 = " << depth << endl;
 
258
    }
 
259
    
 
260
    return depth;
 
261
}
 
262
 
 
263
 
 
264
static double calc_depth2(
 
265
    const Vector3& ip1,
 
266
    const Vector3& ip2,
 
267
    const Vector3& pt1,
 
268
    const Vector3& pt2,
 
269
    const Vector3& pt3,
 
270
    const Vector3& n)
 
271
{
 
272
    // when a triangle penetrates another triangle at two intersecting points
 
273
    // and the separating plane is the supporting plane of the penetrating triangle
 
274
    
 
275
    const Vector3 nn(n); // vecNormalize(nn);
 
276
    
 
277
    const double depth1 = calc_depth(ip1, pt1, pt2, pt3, nn);
 
278
    const double depth2 = calc_depth(ip2, pt1, pt2, pt3, nn);
 
279
    
 
280
    // cout << "depth1 depth2 = " << depth1 << " " << depth2 << endl;
 
281
    const double depth = (depth1 < depth2) ? depth2 : depth1;
 
282
    
 
283
    if(HIRUKAWA_DEBUG){
 
284
        cout << "calc_depth 5 = " << depth << endl;
 
285
    }
 
286
    
 
287
    return depth;
 
288
}
 
289
 
 
290
 
 
291
static void calcNormal(
 
292
    Vector3& vec,
 
293
    const Vector3& v1,
 
294
    const Vector3& v2,
 
295
    const Vector3& v3,
 
296
    double sgn)
 
297
{
 
298
    // find the vector from v1 to the mid point of v2 and v3 when 0<sgn
 
299
    
 
300
    if(sgn < 0){
 
301
        vec = -(v1 - 0.5 * (v2 + v3)).normalized();
 
302
    } else {
 
303
        vec =  (v1 - 0.5 * (v2 + v3)).normalized();
 
304
    }
 
305
}
 
306
 
 
307
 
 
308
static int find_common_perpendicular(
 
309
    const Vector3& p1,
 
310
    const Vector3& p2,
 
311
    const Vector3& q1,
 
312
    const Vector3& q2,
 
313
    const Vector3& ip1,
 
314
    const Vector3& ip2,
 
315
    const Vector3& n1,
 
316
    const Vector3& m1,
 
317
    const Vector3& n_vector,
 
318
    double& dp)
 
319
{
 
320
    const double eps = 1.0e-3; // threshold to consider two edges are parallel
 
321
    const double vn = 1.0e-2;  // threshold to judge an intersecting point is near a vertex
 
322
 
 
323
    const Vector3 e = p2 - p1;
 
324
    const Vector3 f = q2 - q1;
 
325
 
 
326
    const double c11 = e.dot(e);
 
327
    const double c12 = - e.dot(f);
 
328
    const double c21 = - c12;
 
329
    const double c22 = - f.dot(f);
 
330
 
 
331
    const double det = c11 * c22 - c12 * c21;
 
332
    // cout << "det = " << det << endl;
 
333
 
 
334
    if(fabs(det) < eps){
 
335
        return 0;
 
336
    } else {
 
337
        const Vector3 g = q1 - p1;
 
338
        const double a = e.dot(g);
 
339
        const double b = f.dot(g);
 
340
        const double t1 = ( c22 * a - c12 * b) / det;
 
341
        const double t2 = (-c21 * a + c11 * b) / det;
 
342
 
 
343
        // quit if the foot of the common perpendicular is not on an edge
 
344
        if(t1<0 || 1<t1 || t2<0 || 1<t2) return 0;
 
345
 
 
346
        // when two edges are in contact near a vertex of an edge
 
347
        // if(t1<vn || 1.0-vn<t1 || t2<vn || 1.0-vn<t2) return 0;
 
348
 
 
349
        // find_intersection_pt(v1, p1, p2, t1);
 
350
        // find_intersection_pt(v2, q1, q2, t2);
 
351
   
 
352
        dp = calc_depth(ip1, ip2, n_vector); 
 
353
 
 
354
        return 1;
 
355
    }
 
356
}
 
357
    
 
358
 
 
359
// for vertex-face contact
 
360
static inline int get_normal_vector_test(
 
361
    const Vector3& ip1,
 
362
    const Vector3& v1,
 
363
    const Vector3& ip2,
 
364
    const Vector3& v2,
 
365
    const Vector3& n1,
 
366
    const Vector3& m1)
 
367
{
 
368
    // ip1 and ip2 are the intersecting points
 
369
    // v1 and v2 are the vertices of the penetrating triangle
 
370
    // note that (v1-ip1) (v2-ip2) lies on the penetrating triangle
 
371
    
 
372
    // eps_applicability = 0.965926; // Cos(15) threshold to consider two triangles face
 
373
    const double eps_applicability = 0.5; // Cos(60) threshold to consider two triangles face
 
374
    
 
375
    // This condition should be checked mildly because the whole sole of a foot
 
376
    // may sink inside the floor and then no collision is detected.
 
377
    return (eps_applicability < n1.dot(m1)) ? 0 : 1;
 
378
}
 
379
 
 
380
 
 
381
// for edge-edge contact
 
382
static int get_normal_vector_test(
 
383
    Vector3& n_vector,
 
384
    const Vector3& ef0,
 
385
    const Vector3& ip,
 
386
    const Vector3& iq,
 
387
    const Vector3& v1,
 
388
    const Vector3& v2,
 
389
    const Vector3& n1,
 
390
    const Vector3& m1,
 
391
    const Vector3& va1,
 
392
    const Vector3& va2,
 
393
    const Vector3& vb1,
 
394
    const Vector3& vb2)
 
395
{
 
396
    // ip is the intersecting point on triangle p1p2p3
 
397
    // iq is the intersecting point on triangle q1q2q3
 
398
    // v1 is the vertex of p1p2p3 which is not on the intersecting edge
 
399
    // v2 is the vertex of q1q2q3 which is not on the intersecting edge
 
400
    // note that (v1-ip) lies on triangle p1p2p3 and (v2-iq) on q1q2q3
 
401
 
 
402
    const double eps_applicability = 0.0; // 1.52e-2; // threshold to consider two triangles face
 
403
    const double eps_length = 1.0e-3; // 1mm: thereshold to consider the intersecting edge is short
 
404
    const double eps_theta = 1.0e-1;   // threshold to consider cos(theta) is too small 
 
405
 
 
406
    // quit if two triangles does not satifsy the applicability condition
 
407
    // i.e. two triangles do not face each other
 
408
    if(- eps_applicability < n1.dot(m1)) return 0;
 
409
    
 
410
    const double ea_length = (va1 - va2).norm();
 
411
    const double eb_length = (vb1 - vb2).norm();
 
412
    
 
413
    // return the normal vector of a triangle if an intersecting edge is too short
 
414
    if(ea_length < eps_length || eb_length < eps_length){
 
415
        // cout << "edge is too short" << endl;
 
416
        if(ea_length < eb_length) {
 
417
            n_vector = m1;
 
418
        } else {
 
419
            n_vector = -n1;
 
420
        }
 
421
        return 1;
 
422
    }
 
423
 
 
424
    const Vector3 sv1 = v1 - ip;
 
425
    const double sv1_norm = sv1.norm();
 
426
    const Vector3 sv2 = v2 - iq;
 
427
    const double sv2_norm = sv2.norm();
 
428
 
 
429
    if(eps_length < sv1_norm && eps_length < sv2_norm){
 
430
        // quit if two triangles do not satisfy the applicability conditions
 
431
        if(- eps_applicability < sv1.dot(sv2) / (sv1_norm * sv2_norm)){
 
432
            return 0;
 
433
        }
 
434
    }
 
435
 
 
436
    // now neither of two edges is not too short
 
437
    Vector3 ef = ef0.normalized();
 
438
 
 
439
    // Triangle p1p2p3
 
440
    const double theta1 = ef.dot(n1) / n1.norm();
 
441
    const double theta1_abs = fabs(theta1);
 
442
    
 
443
    double theta2;
 
444
    double theta2_abs;
 
445
    if(eps_length < sv1_norm){
 
446
        theta2 = ef.dot(sv1) / sv1_norm;
 
447
        theta2_abs = fabs(theta2);
 
448
    } else {
 
449
        theta2 = 0.0;
 
450
        theta2_abs = 0.0;
 
451
    }
 
452
    
 
453
    // triangle q1q2q3
 
454
    const double theta3 = ef.dot(m1) / m1.norm();
 
455
    const double theta3_abs = fabs(theta3);
 
456
    
 
457
    double theta4;
 
458
    double theta4_abs;
 
459
    if(eps_length < sv2_norm){
 
460
        theta4 = ef.dot(sv2) / sv2_norm;
 
461
        theta4_abs = fabs(theta4);
 
462
    } else {
 
463
        theta4 = 0.0;
 
464
        theta4_abs = 0.0;
 
465
    }
 
466
 
 
467
    if(sv1_norm < eps_length || sv2_norm < eps_length){
 
468
        // when sv1 or sv2 is too short
 
469
        // cout << "sv is too short" << endl;
 
470
        if(theta1_abs < theta3_abs){
 
471
            n_vector = m1;
 
472
        } else {
 
473
            n_vector = -n1;
 
474
        }
 
475
        return 1;    
 
476
    }
 
477
 
 
478
    if(theta2_abs < eps_theta && theta4_abs < eps_theta){
 
479
        // when two triangles are coplanar
 
480
        // proof.
 
481
        //  ef = (va2-va1)x(vb2-vb1) (1)
 
482
        //  sv1 * ef = 0             (2)
 
483
        //  sv2 * ef = 0             
 
484
        //  substituting (1) to (2),
 
485
        //    sv1 * (va2-va1) x (vb2-vb1) = 0
 
486
        //    (vb2 - vb1) * sv1 x (va2 - va1) = 0
 
487
        //  considering sv1 x (va2 - va1) = n,
 
488
        //    (vb2 - vb1) * n = 0
 
489
        //  in the same way
 
490
        //    (va2 - va1) * m = 0
 
491
        // q.e.d.
 
492
 
 
493
        if(theta1_abs < theta3_abs){
 
494
            n_vector = m1;
 
495
            return 1;
 
496
        } else{
 
497
            n_vector = -n1;
 
498
            return 1;
 
499
        }
 
500
    }
 
501
 
 
502
    // return 0 if the plane which passes through ip with normal vector ef
 
503
    // does not separate v1 and v2
 
504
    if(-eps_applicability < theta2 * theta4) return 0;
 
505
 
 
506
    //
 
507
    // regular case
 
508
    //
 
509
    double theta12;
 
510
    if(theta1_abs < theta2_abs){
 
511
        theta12 = theta2;
 
512
    } else {
 
513
        theta12 = -1.0 * theta1;
 
514
    }
 
515
 
 
516
    double theta34;
 
517
    if(theta3_abs < theta4_abs){
 
518
        theta34 = -1.0 * theta4;
 
519
    } else {
 
520
        theta34 = theta3;
 
521
    }
 
522
 
 
523
    double theta;
 
524
    if(fabs(theta12) < fabs(theta34)){
 
525
        theta = theta34;
 
526
    } else {
 
527
        theta = theta12;
 
528
    }
 
529
 
 
530
    if(0 < theta){
 
531
        n_vector = ef;
 
532
    } else {
 
533
        n_vector = -ef;
 
534
    }
 
535
 
 
536
    if(HIRUKAWA_DEBUG){
 
537
        cout << "va1=" << va1[0] << " " << va1[1] << " " << va1[2] << endl;
 
538
        cout << "va2=" << va2[0] << " " << va2[1] << " " << va2[2] << endl;
 
539
        cout << "va3=" << v1[0] << " " << v1[1] << " " << v1[2] << endl;
 
540
        cout << "vb1=" << vb1[0] << " " << vb1[1] << " " << vb1[2] << endl;
 
541
        cout << "vb2=" << vb2[0] << " " << vb2[1] << " " << vb2[2] << endl;
 
542
        cout << "vb3=" << v2[0] << " " << v2[1] << " " << v2[2] << endl;
 
543
        cout << "n1=" << n1[0] << " " << n1[1] << " " << n1[2] << endl;
 
544
        cout << "m1=" << m1[0] << " " << m1[1] << " " << m1[2] << endl;
 
545
        cout << "ef=" << ef[0] << " " << ef[1] << " " << ef[2] << endl;
 
546
        cout << "sv1=" << sv1[0] << " " << sv1[1] << " " << sv1[2] << endl;
 
547
        cout << "sv2=" << sv2[0] << " " << sv2[1] << " " << sv2[2] << endl;
 
548
        cout << endl;
 
549
    }
 
550
 
 
551
    if(n_vector.dot(sv1) < eps_applicability || - eps_applicability < n_vector.dot(sv2)){
 
552
        // when the separating plane separates the outsides of the triangles
 
553
        return 0;
 
554
    } else {
 
555
        return 1;
 
556
    }
 
557
}
 
558
 
 
559
 
 
560
//
 
561
// find the collision info when a vertex penetrates a face
 
562
//
 
563
static int find_collision_info(
 
564
    const Vector3& p1,
 
565
    const Vector3& p2,
 
566
    const Vector3& p3,
 
567
    double mp0,
 
568
    double mp1,
 
569
    double mp2,
 
570
    const Vector3& q1,
 
571
    const Vector3& q2,
 
572
    const Vector3& q3,
 
573
    const Vector3& f1,
 
574
    const Vector3& f2,
 
575
    const Vector3& f3,
 
576
    const Vector3& n1,
 
577
    const Vector3& m1,
 
578
    Vector3& ip3,
 
579
    Vector3& ip4,
 
580
    Vector3& ip5, /* unused ? */
 
581
    Vector3& ip6, /* unused ? */
 
582
    collision_data* col_p, double pq)
 
583
{
 
584
    Vector3 ip1;
 
585
    find_intersection_pt(ip1, p1, p2, mp0, mp1);
 
586
    Vector3 ip2;
 
587
    find_intersection_pt(ip2, p3, p1, mp2, mp0);
 
588
    
 
589
    if(get_normal_vector_test(ip1, p2, ip2, p3, m1, n1)){
 
590
 
 
591
        Vector3 vec;
 
592
        calcNormal(vec, p1, p2, p3, mp0);
 
593
        
 
594
        col_p->n_vector = m1 * pq;
 
595
 
 
596
        //
 
597
        // The depth is estimated in InsertCollisionPair.cpp
 
598
        // The following depth calculation is done only for debugging purpose
 
599
        //
 
600
        col_p->depth = calc_depth(ip1, ip2, p2, p3, p1, col_p->n_vector);
 
601
        const Vector3 nv = -n1 * pq;
 
602
        const double dp = calc_depth2(ip1, ip2, q1, q2, q3, nv);
 
603
        if(dp < col_p->depth){
 
604
            col_p->depth = dp;
 
605
        }
 
606
 
 
607
        ip3 = ip1; ip4 = ip2;
 
608
        col_p->num_of_i_points = 2;
 
609
 
 
610
        return 1;
 
611
    }
 
612
 
 
613
    return 0;
 
614
}
 
615
 
 
616
 
 
617
//
 
618
// find the collision info when an edges penetrate a face each other 
 
619
//
 
620
static int find_collision_info(
 
621
    const Vector3& p1,
 
622
    const Vector3& p2,
 
623
    const Vector3& p3,
 
624
    double mp0,
 
625
    double mp1,
 
626
    const Vector3& q1,
 
627
    const Vector3& q2,
 
628
    const Vector3& q3,
 
629
    double nq0,
 
630
    double nq1,
 
631
    const Vector3& ef11,
 
632
    const Vector3& n1,
 
633
    const Vector3& m1,
 
634
    Vector3& ip3,
 
635
    Vector3& ip4,
 
636
    collision_data *col_p)
 
637
{
 
638
    Vector3 ip1;
 
639
    find_intersection_pt(ip1, q1, q2, nq0, nq1);
 
640
    Vector3 ip2;
 
641
    find_intersection_pt(ip2, p1, p2, mp0, mp1);
 
642
 
 
643
    double dp;
 
644
    if(get_normal_vector_test(col_p->n_vector, ef11, ip2, ip1, p3, q3, n1, m1, p1, p2, q1, q2) &&
 
645
       find_common_perpendicular(p1, p2, q1, q2, ip1, ip2, n1, m1, col_p->n_vector, dp)){
 
646
 
 
647
        ip3 = ip1; ip4 = ip2;
 
648
        col_p->num_of_i_points = 2;
 
649
        col_p->depth = dp;
 
650
        return 1;
 
651
    }
 
652
 
 
653
    return 0;
 
654
}
 
655
 
 
656
 
 
657
// very robust triangle intersection test
 
658
// uses no divisions
 
659
// works on coplanar triangles
 
660
 
 
661
int tri_tri_overlap(
 
662
    const Vector3& P1,
 
663
    const Vector3& P2,
 
664
    const Vector3& P3,
 
665
    const Vector3& Q1,
 
666
    const Vector3& Q2,
 
667
    const Vector3& Q3,
 
668
    collision_data* col_p,
 
669
    CollisionPairInserter* collisionPairInserter) 
 
670
{
 
671
    /*
 
672
      One triangle is (p1,p2,p3).  Other is (q1,q2,q3).
 
673
      Edges are (e1,e2,e3) and (f1,f2,f3).
 
674
      Normals are n1 and m1
 
675
      Outwards are (g1,g2,g3) and (h1,h2,h3).
 
676
     
 
677
      We assume that the triangle vertices are in the same coordinate system.
 
678
 
 
679
      First thing we do is establish a new c.s. so that p1 is at (0,0,0).
 
680
 
 
681
    */
 
682
    Vector3 p1, p2, p3;
 
683
    Vector3 q1, q2, q3;
 
684
    Vector3 e1, e2, e3;
 
685
    Vector3 f1, f2, f3;
 
686
    // Vector3 g1, g2, g3;
 
687
    // Vector3 h1, h2, h3;
 
688
    Vector3 n1, m1;
 
689
    Vector3 z;
 
690
    Vector3 nq, mp;
 
691
 
 
692
    int triP,triQ;
 
693
    int edf1, edf2, edf3, ede1, ede2, ede3;
 
694
 
 
695
    Vector3 ef11, ef12, ef13;
 
696
    Vector3 ef21, ef22, ef23;
 
697
    Vector3 ef31, ef32, ef33;
 
698
 
 
699
    /* intersection point   R is a flag which tri P & Q correspond or not  */
 
700
    Vector3 ip,ip3,ip4,ip5,ip6;
 
701
    Vector3 i_pts_w[4];
 
702
  
 
703
    const int FV = 1; // face-vertex contact type
 
704
    const int VF = 2; // vertex-face contact type
 
705
    const int EE = 3; // edge-edge contact type
 
706
 
 
707
    z.setZero();
 
708
  
 
709
    p1 =  P1 - P1;
 
710
    p2 =  P2 - P1;
 
711
    p3 =  P3 - P1;
 
712
  
 
713
    q1 =  Q1 - P1;
 
714
    q2 =  Q2 - P1;
 
715
    q3 =  Q3 - P1;
 
716
  
 
717
    e1 =  p2 - p1;
 
718
    e2 =  p3 - p2;
 
719
    e3 =  p1 - p3;
 
720
 
 
721
    f1 =  q2 - q1;
 
722
    f2 =  q3 - q2;
 
723
    f3 =  q1 - q3;
 
724
 
 
725
    n1 = e1.cross(e2);
 
726
    m1 = f1.cross(f2);
 
727
 
 
728
    // now begin the series of tests
 
729
 
 
730
    /*************************************
 
731
        separability test by face
 
732
    ************************************/
 
733
 
 
734
    nq[0] = n1.dot(q1);
 
735
    nq[1] = n1.dot(q2);
 
736
    nq[2] = n1.dot(q3);
 
737
    triQ = separability_test_by_face(nq);
 
738
 
 
739
    if(triQ == NOT_INTERSECT) return 0;
 
740
 
 
741
    double mq = m1.dot(q1);
 
742
    mp[0] = m1.dot(p1) - mq;
 
743
    mp[1] = m1.dot(p2) - mq;
 
744
    mp[2] = m1.dot(p3) - mq;
 
745
    triP = separability_test_by_face(mp);
 
746
    if(triP == NOT_INTERSECT) return 0;
 
747
 
 
748
    ef11 = e1.cross(f1);
 
749
    ef12 = e1.cross(f2);
 
750
    ef13 = e1.cross(f3);
 
751
    ef21 = e2.cross(f1);
 
752
    ef22 = e2.cross(f2);
 
753
    ef23 = e2.cross(f3);
 
754
    ef31 = e3.cross(f1);
 
755
    ef32 = e3.cross(f2);
 
756
    ef33 = e3.cross(f3);
 
757
 
 
758
    edf1 = 0; edf2 = 0; edf3 = 0; ede1 = 0; ede2 = 0; ede3 = 0;
 
759
 
 
760
    /********************************
 
761
         triangle inside test
 
762
    *********************************/  
 
763
    switch(triQ)
 
764
        {
 
765
        case NOT_INTERSECT:
 
766
            return 0;
 
767
 
 
768
        case EDGE1_NOT_INTERSECT:    
 
769
            edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2);
 
770
            edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3);
 
771
            break;
 
772
 
 
773
        case EDGE2_NOT_INTERSECT:         
 
774
            edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1);    
 
775
            edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3);
 
776
            break;
 
777
 
 
778
        case EDGE3_NOT_INTERSECT:       
 
779
            edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1);    
 
780
            edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2);
 
781
            break;
 
782
        }
 
783
 
 
784
    int num_of_edges = edf1 + edf2 + edf3;
 
785
    if(num_of_edges == 3){
 
786
        //exit(1);
 
787
        return 0;
 
788
    }
 
789
 
 
790
    if(num_of_edges < 2){
 
791
        switch(triP)
 
792
            {
 
793
            case EDGE1_NOT_INTERSECT:
 
794
                ede2 = triangle_inside_test(ef21, ef22, ef23, q3, q1, q2, p2);
 
795
                ede3 = triangle_inside_test(ef31, ef32, ef33, q3, q1, q2, p3);
 
796
                if(ede2 + ede3 == 2){
 
797
                    edf1 = NOT_INTERSECT;
 
798
                    edf2 = NOT_INTERSECT;
 
799
                    edf3 = NOT_INTERSECT;
 
800
                }
 
801
                break;
 
802
        
 
803
            case EDGE2_NOT_INTERSECT:
 
804
                ede1 = triangle_inside_test(ef11, ef12, ef13, q3, q1, q2, p1);
 
805
                ede3 = triangle_inside_test(ef31, ef32, ef33, q3, q1, q2, p3);
 
806
                if(ede1 + ede3 == 2){
 
807
                    edf1 = NOT_INTERSECT;
 
808
                    edf2 = NOT_INTERSECT;
 
809
                    edf3 = NOT_INTERSECT;
 
810
                }
 
811
                break;    
 
812
        
 
813
            case EDGE3_NOT_INTERSECT:
 
814
                ede1 = triangle_inside_test(ef11, ef12, ef13, q3, q1, q2, p1);
 
815
                ede2 = triangle_inside_test(ef21, ef22, ef23, q3, q1, q2, p2);
 
816
                if(ede1 + ede2 == 2){
 
817
                    edf1 = NOT_INTERSECT;
 
818
                    edf2 = NOT_INTERSECT;
 
819
                    edf3 = NOT_INTERSECT;
 
820
                }
 
821
                break;
 
822
            }
 
823
        if(num_of_edges == 0 && (ede1 + ede2 + ede3) == 3){
 
824
            //exit(1);
 
825
            return 0;
 
826
        }
 
827
    }
 
828
  
 
829
    int num = edf1 + edf2 + edf3 + ede1 + ede2 + ede3;
 
830
    if(num == 0){
 
831
        // cout << "no edge intersect" << endl;
 
832
        return 0;
 
833
    }
 
834
    else if(num > 2){
 
835
        printf("err of edge detection....");
 
836
        //exit(1);
 
837
        return 0;
 
838
    }
 
839
 
 
840
    n1.normalize();
 
841
    m1.normalize();
 
842
 
 
843
    /*********************************
 
844
    find intersection points
 
845
    **********************************/
 
846
    if(num == 1){
 
847
        if(edf1 == INTERSECT){
 
848
            find_intersection_pt(ip, q1, q2, nq[0], nq[1]);
 
849
            ip3 = ip;
 
850
            col_p->n_vector = -n1;
 
851
            col_p->depth = 0.0;
 
852
            col_p->c_type = FV;
 
853
 
 
854
        } else if(edf2 == INTERSECT){
 
855
            find_intersection_pt(ip, q2, q3, nq[1], nq[2]);
 
856
            ip3 = ip;
 
857
            col_p->n_vector = -n1;
 
858
            col_p->depth = 0.0;
 
859
            col_p->c_type = FV;
 
860
 
 
861
        } else if(edf3 == INTERSECT){
 
862
            find_intersection_pt(ip, q3, q1, nq[2], nq[0]);
 
863
            ip3 = ip;
 
864
            col_p->n_vector = -n1;
 
865
            col_p->depth = 0.0;
 
866
            col_p->c_type = FV;
 
867
 
 
868
        } else if(ede1 == INTERSECT){
 
869
            find_intersection_pt(ip, p1, p2, mp[0], mp[1]);
 
870
            ip3 = ip;
 
871
            col_p->n_vector = m1;
 
872
            col_p->depth = 0.0;
 
873
            col_p->c_type = VF;
 
874
 
 
875
        } else if(ede2 == INTERSECT){
 
876
            find_intersection_pt(ip, p2, p3, mp[1], mp[2]);
 
877
            ip3 = ip;
 
878
            col_p->n_vector = m1;
 
879
            col_p->depth = 0.0;
 
880
            col_p->c_type = VF;
 
881
 
 
882
        } else if(ede3 == INTERSECT){
 
883
            find_intersection_pt(ip, p3, p1, mp[2], mp[0]);
 
884
            ip3 = ip;
 
885
            col_p->n_vector = m1;
 
886
            col_p->depth = 0.0;
 
887
            col_p->c_type = VF;
 
888
        }
 
889
        col_p->num_of_i_points = 1;
 
890
 
 
891
    } else if(num == 2) {
 
892
        if(edf1 == INTERSECT && edf2 == INTERSECT){
 
893
            if(HIRUKAWA_DEBUG) cout << "f1 f2" << endl;
 
894
            col_p->c_type = FV;
 
895
            if(!find_collision_info(q2,q1,q3,nq[1],nq[0],nq[2],p1,p2,p3,e1,e2,e3,
 
896
                                    m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){
 
897
                return 0;
 
898
            }
 
899
        } else if(edf1 == INTERSECT && edf3 == INTERSECT){
 
900
            if(HIRUKAWA_DEBUG) cout << "f1 f3" << endl;
 
901
            col_p->c_type = FV;
 
902
            if(!find_collision_info(q1,q2,q3,nq[0],nq[1],nq[2],p1,p2,p3,e1,e2,e3,
 
903
                                    m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){
 
904
                return 0;
 
905
            }
 
906
        } else if(ede1 == INTERSECT && edf1 == INTERSECT){
 
907
            if(HIRUKAWA_DEBUG) cout << "e1 f1" << endl;
 
908
            col_p->c_type = EE;
 
909
            if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q1,q2,q3,nq[0],nq[1],ef11,
 
910
                                    n1,m1,ip3,ip4,col_p)){
 
911
                return 0;
 
912
            }
 
913
        } else if(ede2 == INTERSECT && edf1 == INTERSECT){
 
914
            if(HIRUKAWA_DEBUG) cout << "e2 f1" << endl;
 
915
            col_p->c_type = EE;
 
916
            if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q1,q2,q3,nq[0],nq[1],ef21,
 
917
                                    n1,m1,ip3,ip4,col_p)){
 
918
                return 0;
 
919
            }
 
920
        } else if(ede3 == INTERSECT && edf1 == INTERSECT){
 
921
            if(HIRUKAWA_DEBUG) cout << "e3 f1" << endl;
 
922
            col_p->c_type = EE;
 
923
            if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q1,q2,q3,nq[0],nq[1],ef31,
 
924
                                    n1,m1,ip3,ip4,col_p)){
 
925
                return 0;
 
926
            }
 
927
        } else if(edf2 == INTERSECT && edf3 == INTERSECT){
 
928
            if(HIRUKAWA_DEBUG) cout << "f2 f3" << endl;
 
929
            col_p->c_type = FV;
 
930
            if(!find_collision_info(q3,q2,q1,nq[2],nq[1],nq[0],p1,p2,p3,e1,e2,e3,
 
931
                                    m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0)){
 
932
                return 0;
 
933
            }
 
934
        } else if(ede1 == INTERSECT && edf2 == INTERSECT){
 
935
            if(HIRUKAWA_DEBUG) cout << "e1 f2" << endl;
 
936
            col_p->c_type = EE;
 
937
            if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q2,q3,q1,nq[1],nq[2],ef12,
 
938
                                    n1,m1,ip3,ip4,col_p)){
 
939
                return 0;
 
940
            }
 
941
        } else if(ede2 == INTERSECT && edf2 == INTERSECT){
 
942
            if(HIRUKAWA_DEBUG) cout << "e2 f2" << endl;
 
943
            col_p->c_type = EE;
 
944
            if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q2,q3,q1,nq[1],nq[2],ef22,
 
945
                                    n1,m1,ip3,ip4,col_p)){
 
946
                return 0;
 
947
            }
 
948
        } else if(ede3 == INTERSECT && edf2 == INTERSECT){
 
949
            if(HIRUKAWA_DEBUG) cout << "e3 f2" << endl;
 
950
            col_p->c_type = EE;
 
951
            if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q2,q3,q1,nq[1],nq[2],ef32,
 
952
                                    n1,m1,ip3,ip4,col_p)){
 
953
                return 0;
 
954
            }
 
955
        } else if(ede1 == INTERSECT && edf3 == INTERSECT){
 
956
            if(HIRUKAWA_DEBUG) cout << "e1 f3" << endl;
 
957
            col_p->c_type = EE;
 
958
            if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q3,q1,q2,nq[2],nq[0],ef13,
 
959
                                    n1,m1,ip3,ip4,col_p)){
 
960
                return 0;
 
961
            }
 
962
        } else if(ede2 == INTERSECT && edf3 == INTERSECT){
 
963
            if(HIRUKAWA_DEBUG) cout << "e2 f3" << endl;
 
964
            col_p->c_type = EE;
 
965
            if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q3,q1,q2,nq[2],nq[0],ef23,
 
966
                                    n1,m1,ip3,ip4,col_p)){
 
967
                return 0;
 
968
            }
 
969
        } else if(ede3 == INTERSECT && edf3 == INTERSECT){
 
970
            if(HIRUKAWA_DEBUG) cout << "e3 f3" << endl;
 
971
            col_p->c_type = EE;
 
972
            if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q3,q1,q2,nq[2],nq[0],ef33,
 
973
                                    n1,m1,ip3,ip4,col_p)){
 
974
                return 0;
 
975
            }
 
976
        } else if(ede1 == INTERSECT && ede2 == INTERSECT){
 
977
            if(HIRUKAWA_DEBUG) cout << "e1 e2" << endl;
 
978
            col_p->c_type = VF;
 
979
            if(!find_collision_info(p2,p1,p3,mp[1],mp[0],mp[2],q1,q2,q3,f1,f2,f3,
 
980
                                    n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){
 
981
                return 0;
 
982
            }
 
983
        } else if(ede1 == INTERSECT && ede3 == INTERSECT){
 
984
            if(HIRUKAWA_DEBUG) cout << "e1 e3" << endl;
 
985
            col_p->c_type = VF;
 
986
            if(!find_collision_info(p1,p2,p3,mp[0],mp[1],mp[2],q1,q2,q3,f1,f2,f3,
 
987
                                    n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){
 
988
                return 0;
 
989
            }
 
990
        } else if(ede2 == INTERSECT && ede3 == INTERSECT){
 
991
            if(HIRUKAWA_DEBUG) cout << "e2 e3" << endl;
 
992
            col_p->c_type = VF;
 
993
            if(!find_collision_info(p3,p2,p1,mp[2],mp[1],mp[0],q1,q2,q3,f1,f2,f3,
 
994
                                    n1,m1,ip3,ip4,ip5,ip6,col_p,1.0)){
 
995
                return 0;
 
996
            }
 
997
        }
 
998
    }    
 
999
 
 
1000
    if(col_p->num_of_i_points == 1){
 
1001
        col_p->i_points[0] = ip3 + P1;
 
1002
    }
 
1003
    else if(col_p->num_of_i_points == 2){
 
1004
        col_p->i_points[0] = ip3 + P1;
 
1005
        col_p->i_points[1] = ip4 + P1;
 
1006
    }
 
1007
    else if(col_p->num_of_i_points == 3){
 
1008
        col_p->i_points[0] = ip3 + P1;
 
1009
        col_p->i_points[1] = ip4 + P1;
 
1010
        col_p->i_points[2] = ip5 + P1;
 
1011
    }
 
1012
    else if(col_p->num_of_i_points == 4){
 
1013
        col_p->i_points[0] = ip3 + P1;
 
1014
        col_p->i_points[1] = ip4 + P1;
 
1015
        col_p->i_points[2] = ip5 + P1;
 
1016
        col_p->i_points[3] = ip5 + P1;
 
1017
    }
 
1018
 
 
1019
    col_p->n = n1;
 
1020
    col_p->m = m1;
 
1021
    
 
1022
    if(HIRUKAWA_DEBUG){
 
1023
 
 
1024
        CollisionPairInserter& c = *collisionPairInserter;
 
1025
    
 
1026
        Vector3 p1w = c.CD_s2 * (c.CD_Rot2 * P1 + c.CD_Trans2);
 
1027
        Vector3 p2w = c.CD_s2 * (c.CD_Rot2 * P2 + c.CD_Trans2);
 
1028
        Vector3 p3w = c.CD_s2 * (c.CD_Rot2 * P3 + c.CD_Trans2);
 
1029
        Vector3 q1w = c.CD_s2 * (c.CD_Rot2 * Q1 + c.CD_Trans2);
 
1030
        Vector3 q2w = c.CD_s2 * (c.CD_Rot2 * Q2 + c.CD_Trans2);
 
1031
        Vector3 q3w = c.CD_s2 * (c.CD_Rot2 * Q3 + c.CD_Trans2);
 
1032
        cout << "P1 = " << p1w[0] << " " << p1w[1] << " " << p1w[2] << endl;
 
1033
        cout << "P2 = " << p2w[0] << " " << p2w[1] << " " << p2w[2] << endl;
 
1034
        cout << "P3 = " << p3w[0] << " " << p3w[1] << " " << p3w[2] << endl;
 
1035
        cout << "Q1 = " << q1w[0] << " " << q1w[1] << " " << q1w[2] << endl;
 
1036
        cout << "Q2 = " << q2w[0] << " " << q2w[1] << " " << q2w[2] << endl;
 
1037
        cout << "Q3 = " << q3w[0] << " " << q3w[1] << " " << q3w[2] << endl;
 
1038
 
 
1039
        for(int i=0; i<col_p->num_of_i_points; i++){
 
1040
            i_pts_w[i] = c.CD_s2 * ((c.CD_Rot2 * col_p->i_points[i]) + c.CD_Trans2);
 
1041
            cout << i << "-th intersecting point = ";
 
1042
            cout << i_pts_w[i][0] << " " << i_pts_w[i][1] << " " << i_pts_w[i][2] << endl;
 
1043
        }
 
1044
 
 
1045
        cout << "n1 = " << n1[0] << " " << n1[1] << " " << n1[2] << endl;
 
1046
        cout << "m1 = " << m1[0] << " " << m1[1] << " " << m1[2] << endl;
 
1047
        cout << "mp[0] mp[1] mp[2] = " << mp[0] << " " << mp[1] << " " << mp[2] << endl;
 
1048
        cout << "nq[0] nq[1] nq[2] = " << nq[0] << " " << nq[1] << " " << nq[2] << endl;
 
1049
        cout << "n_vector = " << col_p->n_vector[0] << " " << col_p->n_vector[1]
 
1050
             << " " << col_p->n_vector[2] << endl;
 
1051
        cout << "depth = " << col_p->depth << endl << endl;;
 
1052
 
 
1053
    }
 
1054
 
 
1055
#if TRACE1
 
1056
    printf("intersect point...in tri_contact..\n");
 
1057
    printf("    ip1x = %f ip1y = %f ip1z = %f\n    ip2x = %f ip2y = %f ip2z = %f\n",
 
1058
           col_p->i_points[0][0],col_p->i_points[0][1],col_p->i_points[0][2],
 
1059
           col_p->i_points[1][0],col_p->i_points[1][1],col_p->i_points[1][2]);
 
1060
 
 
1061
    printf("normal vector....it tri_conctact..\n");
 
1062
    printf("N[0] = %f,N[1] = %f,N[2] = %f\n",col_p->n_vector[0],col_p->n_vector[1],col_p->n_vector[2]);
 
1063
#endif
 
1064
 
 
1065
    return 1;
 
1066
}