1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 #include "tokamak.h"
15 #include "containers.h"
16 #include "scenery.h"
17 #include "collision.h"
18 #include "collision2.h"
19 #include "constraint.h"
20 #include "rigidbody.h"
21
22 #include <assert.h>
23 #include <stdio.h>
24
CylinderFaceTest(ConvexTestResult & res,TConvex & cylinderB,neT3 & transB,s32 whichFace)25 neBool BoxTestParam::CylinderFaceTest(ConvexTestResult & res, TConvex & cylinderB, neT3 & transB, s32 whichFace)
26 {
27 neV3 diff = trans->pos - transB.pos;
28
29 neV3 dir = trans->rot[whichFace];
30
31 f32 dot = dir.Dot(diff);
32
33 if (dot > 0.0f)
34 {
35 dot *= -1.0f;
36 }
37 else
38 {
39 dir *= -1.0f;
40 }
41
42 f32 depth = dot + convex->BoxSize(whichFace);
43
44 neV3 contactPoint = transB.pos;
45
46 neV3 tmp = transB.rot[1] * cylinderB.CylinderHalfHeight();
47
48 dot = tmp.Dot(dir);
49
50 if (dot > 0.0f)
51 {
52 depth += dot;
53
54 contactPoint += tmp;
55 }
56 else
57 {
58 depth -= dot;
59
60 contactPoint -= tmp;
61 }
62 depth += cylinderB.CylinderRadius();
63
64 if (depth <= 0.0f)
65 return false;
66
67 if (depth >= res.depth)
68 return true;
69
70 contactPoint += dir * cylinderB.CylinderRadius();
71
72 neV3 project = contactPoint - dir * depth;
73
74 s32 otherAxis1 = neNextDim1[whichFace];
75
76 s32 otherAxis2 = neNextDim2[whichFace];
77
78 neV3 sub = project - trans->pos;
79
80 dot = neAbs(sub.Dot(trans->rot[otherAxis1]));
81
82 if (dot > (convex->BoxSize(otherAxis1) * 1.001f))
83 return true;// not false ???? no it is true!!!
84
85 dot = neAbs(sub.Dot(trans->rot[otherAxis2]));
86
87 if (dot > (convex->BoxSize(otherAxis2) * 1.001f))
88 return true;// not false ???? no it is true!!!
89
90 res.contactA = project;
91
92 res.contactB = contactPoint;
93
94 res.contactNormal = dir;
95
96 res.depth = depth;
97
98 res.valid = true;
99
100 return true;
101 }
102
CylinderEdgeTest(ConvexTestResult & res,TConvex & cylinderB,neT3 & transB,s32 whichEdge)103 neBool BoxTestParam::CylinderEdgeTest(ConvexTestResult & res, TConvex & cylinderB, neT3 & transB, s32 whichEdge)
104 {
105 neV3 diff = trans->pos - transB.pos;
106
107 neV3 dir = trans->rot[whichEdge].Cross(transB.rot[1]);
108
109 f32 len = dir.Length();
110
111 if (neIsConsiderZero(len))
112 return true;
113
114 dir *= (1.0f / len);
115
116 f32 dot = dir.Dot(diff);
117
118 if (dot > 0.0f)
119 {
120 dot *= -1.0f;
121 }
122 else
123 {
124 dir *= -1.0f;
125 }
126
127 f32 depth = dot + cylinderB.CylinderRadius();
128
129 neV3 contactPoint = trans->pos;
130
131 s32 i;
132
133 for (i = 0; i < 3; i++)
134 {
135 if (i == whichEdge)
136 continue;
137
138 dot = dir.Dot(radii[i]);
139
140 if (dot > 0.0f)
141 {
142 depth += dot;
143
144 contactPoint -= radii[i];
145 }
146 else
147 {
148 depth -= dot;
149
150 contactPoint += radii[i];
151 }
152 }
153 if (depth <= 0.0f)
154 return false;
155
156 ConvexTestResult cr;
157
158 cr.edgeA[0] = contactPoint + radii[whichEdge];
159 cr.edgeA[1] = contactPoint - radii[whichEdge];
160 cr.edgeB[0] = transB.pos + transB.rot[1] * cylinderB.CylinderHalfHeight();
161 cr.edgeB[1] = transB.pos - transB.rot[1] * cylinderB.CylinderHalfHeight();
162
163 f32 au, bu;
164
165 // A is the box, B is the cylinder
166
167 cr.ComputerEdgeContactPoint2(au, bu);
168
169 if (cr.depth >= res.depth)
170 return true;
171
172 if (cr.valid)
173 {
174 depth = cylinderB.CylinderRadius() - cr.depth;
175
176 if (depth <= 0.0f)
177 return false;
178
179 if (depth >= res.depth)
180 return true;;
181
182 res.valid = true;
183
184 res.contactNormal = dir;
185
186 res.contactA = cr.contactA;
187
188 res.contactB = cr.contactB + res.contactNormal * cylinderB.CylinderRadius();
189
190 res.depth = depth;
191 }
192 else
193 {
194 // A is the box, B is the cylinder
195
196 if (au > 0.0 && au < 1.0f)
197 {
198 // box edge and cylinder end
199
200 neV3 cylinderVert;
201
202 if (bu <= 0.0f)
203 {
204 cylinderVert = cr.edgeB[0];
205 }
206 else
207 {
208 cylinderVert = cr.edgeB[1];
209 }
210 neV3 project;
211
212 f32 dist = cylinderVert.GetDistanceFromLine2(project, cr.edgeA[0], cr.edgeA[1]);
213
214 f32 depth = cylinderB.CylinderRadius() - dist;
215
216 if (depth <= 0.0f)
217 return true;
218
219 if (depth >= res.depth)
220 return true;
221
222 res.depth = depth;
223 res.valid = true;
224 res.contactNormal = project - cylinderVert;
225 res.contactNormal.Normalize();
226 res.contactA = project;
227 res.contactB = cylinderVert + res.contactNormal * cylinderB.CylinderRadius();
228 }
229 else
230 {
231 neV3 boxVert;
232
233 if (au <= 0.0f)
234 {
235 boxVert = cr.edgeA[0];
236 }
237 else // au >= 1.0f
238 {
239 boxVert = cr.edgeA[1];
240 }
241 if (bu > 0.0f && bu < 1.0f)
242 {
243 // boxVert and cylinder edge
244
245 neV3 project;
246
247 f32 depth = boxVert.GetDistanceFromLine2(project, cr.edgeB[0], cr.edgeB[1]);
248
249 depth = cylinderB.CylinderRadius() - depth;
250
251 if (depth <= 0.0f)
252 return true;
253
254 if (depth >= res.depth)
255 return true;
256
257 res.depth = depth;
258 res.valid = true;
259 res.contactA = boxVert;
260 res.contactNormal = boxVert - project;
261 res.contactNormal.Normalize();
262 res.contactB = project + res.contactNormal * cylinderB.CylinderRadius();
263 }
264 else
265 {
266 // box vert and cylinder end
267
268 neV3 cylinderVert;
269
270 if (bu <= 0.0f)
271 {
272 cylinderVert = cr.edgeB[0];
273 }
274 else
275 {
276 cylinderVert = cr.edgeB[1];
277 }
278 neV3 diff = boxVert - cylinderVert;
279
280 f32 depth = diff.Dot(diff);
281
282 if (depth >= cylinderB.CylinderRadiusSq())
283 return true;
284
285 depth = sqrtf(depth);
286
287 depth = cylinderB.CylinderRadius() - depth;
288
289 if (depth >= res.depth)
290 return true;
291
292 res.depth = depth;
293 res.valid = true;
294 res.contactNormal = diff;
295 res.contactNormal.Normalize();
296 res.contactA = boxVert;
297 res.contactB = cylinderVert + res.contactNormal * cylinderB.CylinderRadius();
298 }
299 }
300 }
301
302 return true;
303 }