1
2 // This is collision detection. If you do another distance test for collision *response*,
3 // if might be useful to simply *skip* the test below completely, and report a collision.
4 // - if sphere-triangle overlap, result is ok
5 // - if they don't, we'll discard them during collision response with a similar test anyway
6 // Overall this approach should run faster.
7
8 // Original code by David Eberly in Magic.
SphereTriOverlap(const Point & vert0,const Point & vert1,const Point & vert2)9 BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
10 {
11 // Stats
12 mNbVolumePrimTests++;
13
14 // Early exit if one of the vertices is inside the sphere
15 Point kDiff = vert2 - mCenter;
16 float fC = kDiff.SquareMagnitude();
17 if(fC <= mRadius2) return TRUE;
18
19 kDiff = vert1 - mCenter;
20 fC = kDiff.SquareMagnitude();
21 if(fC <= mRadius2) return TRUE;
22
23 kDiff = vert0 - mCenter;
24 fC = kDiff.SquareMagnitude();
25 if(fC <= mRadius2) return TRUE;
26
27 // Else do the full distance test
28 Point TriEdge0 = vert1 - vert0;
29 Point TriEdge1 = vert2 - vert0;
30
31 //Point kDiff = vert0 - mCenter;
32 float fA00 = TriEdge0.SquareMagnitude();
33 float fA01 = TriEdge0 | TriEdge1;
34 float fA11 = TriEdge1.SquareMagnitude();
35 float fB0 = kDiff | TriEdge0;
36 float fB1 = kDiff | TriEdge1;
37 //float fC = kDiff.SquareMagnitude();
38 float fDet = fabsf(fA00*fA11 - fA01*fA01);
39 float u = fA01*fB1-fA11*fB0;
40 float v = fA01*fB0-fA00*fB1;
41 float SqrDist;
42
43 if(u + v <= fDet)
44 {
45 if(u < 0.0f)
46 {
47 if(v < 0.0f) // region 4
48 {
49 if(fB0 < 0.0f)
50 {
51 // v = 0.0f;
52 if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
53 else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
54 }
55 else
56 {
57 // u = 0.0f;
58 if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
59 else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
60 else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
61 }
62 }
63 else // region 3
64 {
65 // u = 0.0f;
66 if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
67 else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
68 else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
69 }
70 }
71 else if(v < 0.0f) // region 5
72 {
73 // v = 0.0f;
74 if(fB0>=0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
75 else if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
76 else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
77 }
78 else // region 0
79 {
80 // minimum at interior point
81 if(fDet==0.0f)
82 {
83 // u = 0.0f;
84 // v = 0.0f;
85 SqrDist = MAX_FLOAT;
86 }
87 else
88 {
89 float fInvDet = 1.0f/fDet;
90 u *= fInvDet;
91 v *= fInvDet;
92 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
93 }
94 }
95 }
96 else
97 {
98 float fTmp0, fTmp1, fNumer, fDenom;
99
100 if(u < 0.0f) // region 2
101 {
102 fTmp0 = fA01 + fB0;
103 fTmp1 = fA11 + fB1;
104 if(fTmp1 > fTmp0)
105 {
106 fNumer = fTmp1 - fTmp0;
107 fDenom = fA00-2.0f*fA01+fA11;
108 if(fNumer >= fDenom)
109 {
110 // u = 1.0f;
111 // v = 0.0f;
112 SqrDist = fA00+2.0f*fB0+fC;
113 }
114 else
115 {
116 u = fNumer/fDenom;
117 v = 1.0f - u;
118 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
119 }
120 }
121 else
122 {
123 // u = 0.0f;
124 if(fTmp1 <= 0.0f) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
125 else if(fB1 >= 0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
126 else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
127 }
128 }
129 else if(v < 0.0f) // region 6
130 {
131 fTmp0 = fA01 + fB1;
132 fTmp1 = fA00 + fB0;
133 if(fTmp1 > fTmp0)
134 {
135 fNumer = fTmp1 - fTmp0;
136 fDenom = fA00-2.0f*fA01+fA11;
137 if(fNumer >= fDenom)
138 {
139 // v = 1.0f;
140 // u = 0.0f;
141 SqrDist = fA11+2.0f*fB1+fC;
142 }
143 else
144 {
145 v = fNumer/fDenom;
146 u = 1.0f - v;
147 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
148 }
149 }
150 else
151 {
152 // v = 0.0f;
153 if(fTmp1 <= 0.0f) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
154 else if(fB0 >= 0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
155 else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
156 }
157 }
158 else // region 1
159 {
160 fNumer = fA11 + fB1 - fA01 - fB0;
161 if(fNumer <= 0.0f)
162 {
163 // u = 0.0f;
164 // v = 1.0f;
165 SqrDist = fA11+2.0f*fB1+fC;
166 }
167 else
168 {
169 fDenom = fA00-2.0f*fA01+fA11;
170 if(fNumer >= fDenom)
171 {
172 // u = 1.0f;
173 // v = 0.0f;
174 SqrDist = fA00+2.0f*fB0+fC;
175 }
176 else
177 {
178 u = fNumer/fDenom;
179 v = 1.0f - u;
180 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
181 }
182 }
183 }
184 }
185
186 return fabsf(SqrDist) < mRadius2;
187 }
188