|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 |
|
24 /* |
|
25 * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de ) |
|
26 * |
|
27 * This testing basically comes down to testing the intersection |
|
28 * of the cylinder caps (discs) with the plane. |
|
29 * |
|
30 */ |
|
31 |
|
32 #include <ode/collision.h> |
|
33 #include <ode/matrix.h> |
|
34 #include <ode/rotation.h> |
|
35 #include <ode/odemath.h> |
|
36 #include <ode/objects.h> |
|
37 |
|
38 #include "collision_kernel.h" // for dxGeom |
|
39 |
|
40 int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) |
|
41 { |
|
42 |
|
43 unsigned char* pContactData = (unsigned char*)contact; |
|
44 int GeomCount = 0; // count of used contactgeoms |
|
45 |
|
46 const dReal toleranz = REAL(0.0001f); |
|
47 |
|
48 |
|
49 // Get the properties of the cylinder (length+radius) |
|
50 dReal radius, length; |
|
51 dGeomCylinderGetParams(Cylinder, &radius, &length); |
|
52 dVector3 &cylpos = Cylinder->final_posr->pos; |
|
53 // and the plane |
|
54 dVector4 planevec; |
|
55 dGeomPlaneGetParams(Plane, planevec); |
|
56 dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; |
|
57 |
|
58 dVector3 G1Pos1, G1Pos2, vDir1; |
|
59 vDir1[0] = Cylinder->final_posr->R[2]; |
|
60 vDir1[1] = Cylinder->final_posr->R[6]; |
|
61 vDir1[2] = Cylinder->final_posr->R[10]; |
|
62 |
|
63 dReal s; |
|
64 s = dMUL(length,REAL(0.5)); |
|
65 G1Pos2[0] = dMUL(vDir1[0],s) + cylpos[0]; |
|
66 G1Pos2[1] = dMUL(vDir1[1],s) + cylpos[1]; |
|
67 G1Pos2[2] = dMUL(vDir1[2],s) + cylpos[2]; |
|
68 |
|
69 G1Pos1[0] = dMUL(vDir1[0],-s) + cylpos[0]; |
|
70 G1Pos1[1] = dMUL(vDir1[1],-s) + cylpos[1]; |
|
71 G1Pos1[2] = dMUL(vDir1[2],-s) + cylpos[2]; |
|
72 |
|
73 dVector3 C; |
|
74 |
|
75 // parallel-check |
|
76 s = dMUL(vDir1[0],PlaneNormal[0]) + dMUL(vDir1[1],PlaneNormal[1]) + dMUL(vDir1[2],PlaneNormal[2]); |
|
77 if(s < 0) |
|
78 s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel |
|
79 else |
|
80 s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel |
|
81 if(s < toleranz && s > (-toleranz)) |
|
82 { |
|
83 // discs are parallel to the plane |
|
84 |
|
85 // 1.compute if, and where contacts are |
|
86 dVector3 P; |
|
87 s = planevec[3] - dMUL(planevec[0],G1Pos1[0]) - dMUL(planevec[1],G1Pos1[1]) - dMUL(planevec[2],G1Pos1[2]); |
|
88 dReal t; |
|
89 t = planevec[3] - dMUL(planevec[0],G1Pos2[0]) - dMUL(planevec[1],G1Pos2[1]) - dMUL(planevec[2],G1Pos2[2]); |
|
90 if(s >= t) // s == t does never happen, |
|
91 { |
|
92 if(s >= 0) |
|
93 { |
|
94 // 1. Disc |
|
95 P[0] = G1Pos1[0]; |
|
96 P[1] = G1Pos1[1]; |
|
97 P[2] = G1Pos1[2]; |
|
98 } |
|
99 else |
|
100 return GeomCount; // no contacts |
|
101 } |
|
102 else |
|
103 { |
|
104 if(t >= 0) |
|
105 { |
|
106 // 2. Disc |
|
107 P[0] = G1Pos2[0]; |
|
108 P[1] = G1Pos2[1]; |
|
109 P[2] = G1Pos2[2]; |
|
110 } |
|
111 else |
|
112 return GeomCount; // no contacts |
|
113 } |
|
114 |
|
115 // 2. generate a coordinate-system on the disc |
|
116 dVector3 V1, V2; |
|
117 if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) |
|
118 { |
|
119 // not x-axis |
|
120 V1[0] = vDir1[0] + REAL(1.0); // random value |
|
121 V1[1] = vDir1[1]; |
|
122 V1[2] = vDir1[2]; |
|
123 } |
|
124 else |
|
125 { |
|
126 // maybe x-axis |
|
127 V1[0] = vDir1[0]; |
|
128 V1[1] = vDir1[1] + REAL(1.0); // random value |
|
129 V1[2] = vDir1[2]; |
|
130 } |
|
131 // V1 is now another direction than vDir1 |
|
132 // Cross-product |
|
133 V2[0] = dMUL(V1[1],vDir1[2]) - dMUL(V1[2],vDir1[1]); |
|
134 V2[1] = dMUL(V1[2],vDir1[0]) - dMUL(V1[0],vDir1[2]); |
|
135 V2[2] = dMUL(V1[0],vDir1[1]) - dMUL(V1[1],vDir1[0]); |
|
136 // make unit V2 |
|
137 t = dSqrt(dMUL(V2[0],V2[0]) + dMUL(V2[1],V2[1]) + dMUL(V2[2],V2[2])); |
|
138 t = dDIV(radius,t); |
|
139 V2[0] = dMUL(V2[0],t); |
|
140 V2[1] = dMUL(V2[1],t); |
|
141 V2[2] = dMUL(V2[2],t); |
|
142 // cross again |
|
143 V1[0] = dMUL(V2[1],vDir1[2]) - dMUL(V2[2],vDir1[1]); |
|
144 V1[1] = dMUL(V2[2],vDir1[0]) - dMUL(V2[0],vDir1[2]); |
|
145 V1[2] = dMUL(V2[0],vDir1[1]) - dMUL(V2[1],vDir1[0]); |
|
146 // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' |
|
147 // V1 = first axis |
|
148 // V2 = second axis |
|
149 |
|
150 // 3. generate contactpoints |
|
151 |
|
152 // Potential contact 1 |
|
153 contact->pos[0] = P[0] + V1[0]; |
|
154 contact->pos[1] = P[1] + V1[1]; |
|
155 contact->pos[2] = P[2] + V1[2]; |
|
156 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
157 if(contact->depth > 0) |
|
158 { |
|
159 contact->normal[0] = PlaneNormal[0]; |
|
160 contact->normal[1] = PlaneNormal[1]; |
|
161 contact->normal[2] = PlaneNormal[2]; |
|
162 contact->g1 = Cylinder; |
|
163 contact->g2 = Plane; |
|
164 GeomCount++; |
|
165 if( GeomCount >= (flags & 0x0ffff)) |
|
166 return GeomCount; // enough contactgeoms |
|
167 pContactData += skip; |
|
168 contact = (dContactGeom*)pContactData; |
|
169 } |
|
170 |
|
171 // Potential contact 2 |
|
172 contact->pos[0] = P[0] - V1[0]; |
|
173 contact->pos[1] = P[1] - V1[1]; |
|
174 contact->pos[2] = P[2] - V1[2]; |
|
175 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
176 if(contact->depth > 0) |
|
177 { |
|
178 contact->normal[0] = PlaneNormal[0]; |
|
179 contact->normal[1] = PlaneNormal[1]; |
|
180 contact->normal[2] = PlaneNormal[2]; |
|
181 contact->g1 = Cylinder; |
|
182 contact->g2 = Plane; |
|
183 GeomCount++; |
|
184 if( GeomCount >= (flags & 0x0ffff)) |
|
185 return GeomCount; // enough contactgeoms |
|
186 pContactData += skip; |
|
187 contact = (dContactGeom*)pContactData; |
|
188 } |
|
189 |
|
190 // Potential contact 3 |
|
191 contact->pos[0] = P[0] + V2[0]; |
|
192 contact->pos[1] = P[1] + V2[1]; |
|
193 contact->pos[2] = P[2] + V2[2]; |
|
194 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
195 if(contact->depth > 0) |
|
196 { |
|
197 contact->normal[0] = PlaneNormal[0]; |
|
198 contact->normal[1] = PlaneNormal[1]; |
|
199 contact->normal[2] = PlaneNormal[2]; |
|
200 contact->g1 = Cylinder; |
|
201 contact->g2 = Plane; |
|
202 GeomCount++; |
|
203 if( GeomCount >= (flags & 0x0ffff)) |
|
204 return GeomCount; // enough contactgeoms |
|
205 pContactData += skip; |
|
206 contact = (dContactGeom*)pContactData; |
|
207 } |
|
208 |
|
209 // Potential contact 4 |
|
210 contact->pos[0] = P[0] - V2[0]; |
|
211 contact->pos[1] = P[1] - V2[1]; |
|
212 contact->pos[2] = P[2] - V2[2]; |
|
213 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
214 if(contact->depth > 0) |
|
215 { |
|
216 contact->normal[0] = PlaneNormal[0]; |
|
217 contact->normal[1] = PlaneNormal[1]; |
|
218 contact->normal[2] = PlaneNormal[2]; |
|
219 contact->g1 = Cylinder; |
|
220 contact->g2 = Plane; |
|
221 GeomCount++; |
|
222 if( GeomCount >= (flags & 0x0ffff)) |
|
223 return GeomCount; // enough contactgeoms |
|
224 pContactData += skip; |
|
225 contact = (dContactGeom*)pContactData; |
|
226 } |
|
227 } |
|
228 else |
|
229 { |
|
230 dReal t = -(dMUL((-PlaneNormal[0]),vDir1[0]) + dMUL((-PlaneNormal[1]),vDir1[1]) + dMUL((-PlaneNormal[2]),vDir1[2])); |
|
231 C[0] = dMUL(vDir1[0],t) - PlaneNormal[0]; |
|
232 C[1] = dMUL(vDir1[1],t) - PlaneNormal[1]; |
|
233 C[2] = dMUL(vDir1[2],t) - PlaneNormal[2]; |
|
234 s = dSqrt(dMUL(C[0],C[0]) + dMUL(C[1],C[1]) + dMUL(C[2],C[2])); |
|
235 // move C onto the circle |
|
236 s = dDIV(radius,s); |
|
237 C[0] = dMUL(C[0],s); |
|
238 C[1] = dMUL(C[1],s); |
|
239 C[2] = dMUL(C[2],s); |
|
240 |
|
241 // deepest point of disc 1 |
|
242 contact->pos[0] = C[0] + G1Pos1[0]; |
|
243 contact->pos[1] = C[1] + G1Pos1[1]; |
|
244 contact->pos[2] = C[2] + G1Pos1[2]; |
|
245 |
|
246 // depth of the deepest point |
|
247 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
248 if(contact->depth >= 0) |
|
249 { |
|
250 contact->normal[0] = PlaneNormal[0]; |
|
251 contact->normal[1] = PlaneNormal[1]; |
|
252 contact->normal[2] = PlaneNormal[2]; |
|
253 contact->g1 = Cylinder; |
|
254 contact->g2 = Plane; |
|
255 GeomCount++; |
|
256 if( GeomCount >= (flags & 0x0ffff)) |
|
257 return GeomCount; // enough contactgeoms |
|
258 pContactData += skip; |
|
259 contact = (dContactGeom*)pContactData; |
|
260 } |
|
261 |
|
262 // C is still computed |
|
263 |
|
264 // deepest point of disc 2 |
|
265 contact->pos[0] = C[0] + G1Pos2[0]; |
|
266 contact->pos[1] = C[1] + G1Pos2[1]; |
|
267 contact->pos[2] = C[2] + G1Pos2[2]; |
|
268 |
|
269 // depth of the deepest point |
|
270 contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); |
|
271 if(contact->depth >= 0) |
|
272 { |
|
273 contact->normal[0] = PlaneNormal[0]; |
|
274 contact->normal[1] = PlaneNormal[1]; |
|
275 contact->normal[2] = PlaneNormal[2]; |
|
276 contact->g1 = Cylinder; |
|
277 contact->g2 = Plane; |
|
278 GeomCount++; |
|
279 if( GeomCount >= (flags & 0x0ffff)) |
|
280 return GeomCount; // enough contactgeoms |
|
281 pContactData += skip; |
|
282 contact = (dContactGeom*)pContactData; |
|
283 } |
|
284 } |
|
285 return GeomCount; |
|
286 } |