← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Collision/Opcode/OPC_TriTriOverlap.h

  • 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/Opcode/OPC_TriTriOverlap.h
 
1
 
 
2
//! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|<EPSILON then dv=0.0;) else no check is done (which is less robust, but faster)
 
3
#define LOCAL_EPSILON 0.000001f
 
4
 
 
5
//! sort so that a<=b
 
6
#define SORT(a,b)                       \
 
7
        if(a>b)                                 \
 
8
        {                                               \
 
9
                const float c=a;        \
 
10
                a=b;                            \
 
11
                b=c;                            \
 
12
        }
 
13
 
 
14
//! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
 
15
#define EDGE_EDGE_TEST(V0, U0, U1)                                              \
 
16
        Bx = U0[i0] - U1[i0];                                                           \
 
17
        By = U0[i1] - U1[i1];                                                           \
 
18
        Cx = V0[i0] - U0[i0];                                                           \
 
19
        Cy = V0[i1] - U0[i1];                                                           \
 
20
        f  = Ay*Bx - Ax*By;                                                                     \
 
21
        d  = By*Cx - Bx*Cy;                                                                     \
 
22
        if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f))  \
 
23
        {                                                                                                       \
 
24
                const float e=Ax*Cy - Ay*Cx;                                    \
 
25
                if(f>0.0f)                                                                              \
 
26
                {                                                                                               \
 
27
                        if(e>=0.0f && e<=f) return TRUE;                        \
 
28
                }                                                                                               \
 
29
                else                                                                                    \
 
30
                {                                                                                               \
 
31
                        if(e<=0.0f && e>=f) return TRUE;                        \
 
32
                }                                                                                               \
 
33
        }
 
34
 
 
35
//! TO BE DOCUMENTED
 
36
#define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2)              \
 
37
{                                                                                                               \
 
38
        float Bx,By,Cx,Cy,d,f;                                                          \
 
39
        const float Ax = V1[i0] - V0[i0];                                       \
 
40
        const float Ay = V1[i1] - V0[i1];                                       \
 
41
        /* test edge U0,U1 against V0,V1 */                                     \
 
42
        EDGE_EDGE_TEST(V0, U0, U1);                                                     \
 
43
        /* test edge U1,U2 against V0,V1 */                                     \
 
44
        EDGE_EDGE_TEST(V0, U1, U2);                                                     \
 
45
        /* test edge U2,U1 against V0,V1 */                                     \
 
46
        EDGE_EDGE_TEST(V0, U2, U0);                                                     \
 
47
}
 
48
 
 
49
//! TO BE DOCUMENTED
 
50
#define POINT_IN_TRI(V0, U0, U1, U2)                                    \
 
51
{                                                                                                               \
 
52
        /* is T1 completly inside T2? */                                        \
 
53
        /* check if V0 is inside tri(U0,U1,U2) */                       \
 
54
        float a  = U1[i1] - U0[i1];                                                     \
 
55
        float b  = -(U1[i0] - U0[i0]);                                          \
 
56
        float c  = -a*U0[i0] - b*U0[i1];                                        \
 
57
        float d0 = a*V0[i0] + b*V0[i1] + c;                                     \
 
58
                                                                                                                \
 
59
        a  = U2[i1] - U1[i1];                                                           \
 
60
        b  = -(U2[i0] - U1[i0]);                                                        \
 
61
        c  = -a*U1[i0] - b*U1[i1];                                                      \
 
62
        const float d1 = a*V0[i0] + b*V0[i1] + c;                       \
 
63
                                                                                                                \
 
64
        a  = U0[i1] - U2[i1];                                                           \
 
65
        b  = -(U0[i0] - U2[i0]);                                                        \
 
66
        c  = -a*U2[i0] - b*U2[i1];                                                      \
 
67
        const float d2 = a*V0[i0] + b*V0[i1] + c;                       \
 
68
        if(d0*d1>0.0f)                                                                          \
 
69
        {                                                                                                       \
 
70
                if(d0*d2>0.0f) return TRUE;                                             \
 
71
        }                                                                                                       \
 
72
}
 
73
 
 
74
//! TO BE DOCUMENTED
 
75
BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
 
76
{
 
77
        float A[3];
 
78
        short i0,i1;
 
79
        /* first project onto an axis-aligned plane, that maximizes the area */
 
80
        /* of the triangles, compute indices: i0,i1. */
 
81
        A[0] = fabsf(n[0]);
 
82
        A[1] = fabsf(n[1]);
 
83
        A[2] = fabsf(n[2]);
 
84
        if(A[0]>A[1])
 
85
        {
 
86
                if(A[0]>A[2])
 
87
                {
 
88
                        i0=1;      /* A[0] is greatest */
 
89
                        i1=2;
 
90
                }
 
91
                else
 
92
                {
 
93
                        i0=0;      /* A[2] is greatest */
 
94
                        i1=1;
 
95
                }
 
96
        }
 
97
        else   /* A[0]<=A[1] */
 
98
        {
 
99
                if(A[2]>A[1])
 
100
                {
 
101
                        i0=0;      /* A[2] is greatest */
 
102
                        i1=1;
 
103
                }
 
104
                else
 
105
                {
 
106
                        i0=0;      /* A[1] is greatest */
 
107
                        i1=2;
 
108
                }
 
109
        }
 
110
 
 
111
        /* test all edges of triangle 1 against the edges of triangle 2 */
 
112
        EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
 
113
        EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
 
114
        EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
 
115
 
 
116
        /* finally, test if tri1 is totally contained in tri2 or vice versa */
 
117
        POINT_IN_TRI(v0, u0, u1, u2);
 
118
        POINT_IN_TRI(u0, v0, v1, v2);
 
119
 
 
120
        return FALSE;
 
121
}
 
122
 
 
123
//! TO BE DOCUMENTED
 
124
#define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1)    \
 
125
{                                                                                                                                                                               \
 
126
        if(D0D1>0.0f)                                                                                                                                           \
 
127
        {                                                                                                                                                                       \
 
128
                /* here we know that D0D2<=0.0 */                                                                                               \
 
129
                /* that is D0, D1 are on the same side, D2 on the other or on the plane */              \
 
130
                A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
 
131
        }                                                                                                                                                                       \
 
132
        else if(D0D2>0.0f)                                                                                                                                      \
 
133
        {                                                                                                                                                                       \
 
134
                /* here we know that d0d1<=0.0 */                                                                                               \
 
135
                A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
 
136
        }                                                                                                                                                                       \
 
137
        else if(D1*D2>0.0f || D0!=0.0f)                                                                                                         \
 
138
        {                                                                                                                                                                       \
 
139
                /* here we know that d0d1<=0.0 or that D0!=0.0 */                                                               \
 
140
                A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2;                              \
 
141
        }                                                                                                                                                                       \
 
142
        else if(D1!=0.0f)                                                                                                                                       \
 
143
        {                                                                                                                                                                       \
 
144
                A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
 
145
        }                                                                                                                                                                       \
 
146
        else if(D2!=0.0f)                                                                                                                                       \
 
147
        {                                                                                                                                                                       \
 
148
                A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
 
149
        }                                                                                                                                                                       \
 
150
        else                                                                                                                                                            \
 
151
        {                                                                                                                                                                       \
 
152
                /* triangles are coplanar */                                                                                                    \
 
153
                return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2);                                                              \
 
154
        }                                                                                                                                                                       \
 
155
}
 
156
 
 
157
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
158
/**
 
159
 *      Triangle/triangle intersection test routine,
 
160
 *      by Tomas Moller, 1997.
 
161
 *      See article "A Fast Triangle-Triangle Intersection Test",
 
162
 *      Journal of Graphics Tools, 2(2), 1997
 
163
 *
 
164
 *      Updated June 1999: removed the divisions -- a little faster now!
 
165
 *      Updated October 1999: added {} to CROSS and SUB macros 
 
166
 *
 
167
 *      int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
 
168
 *                      float U0[3],float U1[3],float U2[3])
 
169
 *
 
170
 *      \param          V0              [in] triangle 0, vertex 0
 
171
 *      \param          V1              [in] triangle 0, vertex 1
 
172
 *      \param          V2              [in] triangle 0, vertex 2
 
173
 *      \param          U0              [in] triangle 1, vertex 0
 
174
 *      \param          U1              [in] triangle 1, vertex 1
 
175
 *      \param          U2              [in] triangle 1, vertex 2
 
176
 *      \return         true if triangles overlap
 
177
 */
 
178
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
179
BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
 
180
{
 
181
 
 
182
        // Stats
 
183
        mNbPrimPrimTests++;
 
184
 
 
185
        // Modified by S-cubed, Inc.
 
186
        // Detect collisions by using TriOverlap.cpp instead of the collision detection by OPCODE
 
187
 
 
188
        cnoid::collision_data c_pair;
 
189
        cnoid::Vector3 i0, i1, i2;
 
190
        cnoid::Vector3 p0, p1, p2;
 
191
 
 
192
        // Convert coordinates to match the interface of tri_tri_overlap()@TriOerlap.cpp
 
193
        // Ice/IcePointer => Vector3
 
194
 
 
195
        i0[0] = V0.x;        i0[1] = V0.y;        i0[2] = V0.z;
 
196
        i1[0] = V1.x;        i1[1] = V1.y;        i1[2] = V1.z;
 
197
        i2[0] = V2.x;        i2[1] = V2.y;        i2[2] = V2.z;
 
198
 
 
199
        p0[0] = U0.x;        p0[1] = U0.y;        p0[2] = U0.z;
 
200
        p1[0] = U1.x;        p1[1] = U1.y;        p1[2] = U1.z;
 
201
        p2[0] = U2.x;        p2[1] = U2.y;        p2[2] = U2.z;
 
202
        
 
203
        if(collisionPairInserter &&
 
204
           collisionPairInserter->detectTriTriOverlap(i0, i1, i2, p0, p1, p2, &c_pair)){
 
205
            /*
 
206
                insert_collision_pair(mNowNode0, mNowNode1, mId0, mId1,
 
207
                                      c_pair.num_of_i_points,
 
208
                                      c_pair.i_points,
 
209
                                      c_pair.n_vector,
 
210
                                      c_pair.depth,
 
211
                                      c_pair.n,
 
212
                                      c_pair.m,
 
213
                                      c_pair.c_type,
 
214
                                      (MeshInterface*)mIMesh0,
 
215
                                      (MeshInterface*)mIMesh1);
 
216
            */
 
217
            collisionPairInserter->apply(mNowNode0, mNowNode1, mId0, mId1,
 
218
                                         c_pair.num_of_i_points,
 
219
                                         c_pair.i_points,
 
220
                                         c_pair.n_vector,
 
221
                                         c_pair.depth,
 
222
                                         c_pair.n,
 
223
                                         c_pair.m,
 
224
                                         c_pair.c_type,
 
225
                                         (MeshInterface*)mIMesh0,
 
226
                                         (MeshInterface*)mIMesh1);
 
227
            return TRUE;
 
228
        }
 
229
        return FALSE;
 
230
}