|
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 * * |
|
26 * cylinder-sphere collider by Christoph Beyer (boernerb@web.de) * |
|
27 * * |
|
28 * In Cylinder/Sphere-collisions, there are three possibilies: * |
|
29 * 1. collision with the cylinder's nappe * |
|
30 * 2. collision with one of the cylinder's disc * |
|
31 * 3. collision with one of the disc's border * |
|
32 * * |
|
33 * This collider computes two distances (s, t) and based on them, * |
|
34 * it decides, which collision we have. * |
|
35 * This collider always generates 1 (or 0, if we have no collison) * |
|
36 * contacts. * |
|
37 * It is able to "separate" cylinder and sphere in all * |
|
38 * configurations, but it never pays attention to velocity. * |
|
39 * So, in extrem situations, "tunneling-effect" is possible. * |
|
40 * * |
|
41 *******************************************************************/ |
|
42 |
|
43 #include <ode/collision.h> |
|
44 #include <ode/matrix.h> |
|
45 #include <ode/rotation.h> |
|
46 #include <ode/odemath.h> |
|
47 #include <ode/objects.h> |
|
48 #include "collision_kernel.h" // for dxGeom |
|
49 |
|
50 int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, |
|
51 int /*flags*/, dContactGeom *contact, int /*skip*/) |
|
52 { |
|
53 int GeomCount = 0; // count of used contacts |
|
54 |
|
55 const dReal toleranz = REAL(0.0001f); |
|
56 |
|
57 // get the data from the geoms |
|
58 dReal radius, length; |
|
59 dGeomCylinderGetParams(Cylinder, &radius, &length); |
|
60 dVector3 &cylpos = Cylinder->final_posr->pos; |
|
61 const dReal* pfRot1 = dGeomGetRotation(Cylinder); |
|
62 |
|
63 dReal radius2; |
|
64 radius2 = dGeomSphereGetRadius(Sphere); |
|
65 const dReal* SpherePos = dGeomGetPosition(Sphere); |
|
66 |
|
67 // G1Pos1 is the middle of the first disc |
|
68 // G1Pos2 is the middle of the second disc |
|
69 // vDir1 is the unit direction of the cylinderaxis |
|
70 dVector3 G1Pos1, G1Pos2, vDir1; |
|
71 vDir1[0] = Cylinder->final_posr->R[2]; |
|
72 vDir1[1] = Cylinder->final_posr->R[6]; |
|
73 vDir1[2] = Cylinder->final_posr->R[10]; |
|
74 |
|
75 dReal s; |
|
76 s = dMUL(length,REAL(0.5)); // just a precomputed factor |
|
77 G1Pos2[0] = dMUL(vDir1[0],s) + cylpos[0]; |
|
78 G1Pos2[1] = dMUL(vDir1[1],s) + cylpos[1]; |
|
79 G1Pos2[2] = dMUL(vDir1[2],s) + cylpos[2]; |
|
80 |
|
81 G1Pos1[0] = dMUL(vDir1[0],-s) + cylpos[0]; |
|
82 G1Pos1[1] = dMUL(vDir1[1],-s) + cylpos[1]; |
|
83 G1Pos1[2] = dMUL(vDir1[2],-s) + cylpos[2]; |
|
84 |
|
85 dVector3 C; |
|
86 dReal t; |
|
87 // Step 1: compute the two distances 's' and 't' |
|
88 // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle |
|
89 s = dMUL((SpherePos[0] - G1Pos1[0]),vDir1[0]) - dMUL((G1Pos1[1] - SpherePos[1]),vDir1[1]) - dMUL((G1Pos1[2] - SpherePos[2]),vDir1[2]); |
|
90 if(s < (-radius2) || s > (length + radius2) ) |
|
91 { |
|
92 // Sphere is too far away from the discs |
|
93 // no collision |
|
94 return 0; |
|
95 } |
|
96 |
|
97 // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis |
|
98 C[0] = dMUL(s,vDir1[0]) + G1Pos1[0] - SpherePos[0]; |
|
99 C[1] = dMUL(s,vDir1[1]) + G1Pos1[1] - SpherePos[1]; |
|
100 C[2] = dMUL(s,vDir1[2]) + G1Pos1[2] - SpherePos[2]; |
|
101 // t is the distance from the Sphere-middle to the cylinder-axis! |
|
102 t = dSqrt(dMUL(C[0],C[0]) + dMUL(C[1],C[1]) + dMUL(C[2],C[2]) ); |
|
103 if(t > (radius + radius2) ) |
|
104 { |
|
105 // Sphere is too far away from the cylinder axis! |
|
106 // no collision |
|
107 return 0; |
|
108 } |
|
109 |
|
110 // decide which kind of collision we have: |
|
111 if(t > radius && (s < 0 || s > length) ) |
|
112 { |
|
113 // 3. collision |
|
114 if(s <= 0) |
|
115 { |
|
116 contact->depth = radius2 - dSqrt( dMUL(s,s) + dMUL((t - radius),(t - radius)) ); |
|
117 if(contact->depth < 0) |
|
118 { |
|
119 // no collision! |
|
120 return 0; |
|
121 } |
|
122 contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos1[0]; |
|
123 contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos1[1]; |
|
124 contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos1[2]; |
|
125 contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth)); |
|
126 contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth)); |
|
127 contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth)); |
|
128 contact->g1 = Cylinder; |
|
129 contact->g2 = Sphere; |
|
130 GeomCount++; |
|
131 return GeomCount; |
|
132 } |
|
133 else |
|
134 { |
|
135 // now s is bigger than length here! |
|
136 contact->depth = radius2 - dSqrt( dMUL((s - length),(s - length)) + dMUL((t - radius),(t - radius)) ); |
|
137 if(contact->depth < 0) |
|
138 { |
|
139 // no collision! |
|
140 return 0; |
|
141 } |
|
142 contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos2[0]; |
|
143 contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos2[1]; |
|
144 contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos2[2]; |
|
145 contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth)); |
|
146 contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth)); |
|
147 contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth)); |
|
148 contact->g1 = Cylinder; |
|
149 contact->g2 = Sphere; |
|
150 GeomCount++; |
|
151 return GeomCount; |
|
152 } |
|
153 } |
|
154 else if( (radius - t) <= s && (radius - t) <= (length - s) ) |
|
155 { |
|
156 // 1. collsision |
|
157 if(t > (radius2 + toleranz)) |
|
158 { |
|
159 // cylinder-axis is outside the sphere |
|
160 contact->depth = (radius2 + radius) - t; |
|
161 if(contact->depth < 0) |
|
162 { |
|
163 // should never happen, but just for safeness |
|
164 return 0; |
|
165 } |
|
166 else |
|
167 { |
|
168 C[0] = dDIV(C[0],t); |
|
169 C[1] = dDIV(C[1],t); |
|
170 C[2] = dDIV(C[2],t); |
|
171 contact->pos[0] = dMUL(C[0],radius2) + SpherePos[0]; |
|
172 contact->pos[1] = dMUL(C[1],radius2) + SpherePos[1]; |
|
173 contact->pos[2] = dMUL(C[2],radius2) + SpherePos[2]; |
|
174 contact->normal[0] = C[0]; |
|
175 contact->normal[1] = C[1]; |
|
176 contact->normal[2] = C[2]; |
|
177 contact->g1 = Cylinder; |
|
178 contact->g2 = Sphere; |
|
179 GeomCount++; |
|
180 return GeomCount; |
|
181 } |
|
182 } |
|
183 else |
|
184 { |
|
185 // cylinder-axis is outside of the sphere |
|
186 contact->depth = (radius2 + radius) - t; |
|
187 if(contact->depth < 0) |
|
188 { |
|
189 // should never happen, but just for safeness |
|
190 return 0; |
|
191 } |
|
192 else |
|
193 { |
|
194 contact->pos[0] = C[0] + SpherePos[0]; |
|
195 contact->pos[1] = C[1] + SpherePos[1]; |
|
196 contact->pos[2] = C[2] + SpherePos[2]; |
|
197 contact->normal[0] = dDIV(C[0],t); |
|
198 contact->normal[1] = dDIV(C[1],t); |
|
199 contact->normal[2] = dDIV(C[2],t); |
|
200 contact->g1 = Cylinder; |
|
201 contact->g2 = Sphere; |
|
202 GeomCount++; |
|
203 return GeomCount; |
|
204 } |
|
205 } |
|
206 } |
|
207 else |
|
208 { |
|
209 // 2. collision |
|
210 if(s <= dMUL(length,REAL(0.5)) ) |
|
211 { |
|
212 // collsision with the first disc |
|
213 contact->depth = s + radius2; |
|
214 if(contact->depth < 0) |
|
215 { |
|
216 // should never happen, but just for safeness |
|
217 return 0; |
|
218 } |
|
219 contact->pos[0] = dMUL(radius2,vDir1[0]) + SpherePos[0]; |
|
220 contact->pos[1] = dMUL(radius2,vDir1[1]) + SpherePos[1]; |
|
221 contact->pos[2] = dMUL(radius2,vDir1[2]) + SpherePos[2]; |
|
222 contact->normal[0] = vDir1[0]; |
|
223 contact->normal[1] = vDir1[1]; |
|
224 contact->normal[2] = vDir1[2]; |
|
225 contact->g1 = Cylinder; |
|
226 contact->g2 = Sphere; |
|
227 GeomCount++; |
|
228 return GeomCount; |
|
229 } |
|
230 else |
|
231 { |
|
232 // collsision with the second disc |
|
233 contact->depth = (radius2 + length - s); |
|
234 if(contact->depth < 0) |
|
235 { |
|
236 // should never happen, but just for safeness |
|
237 return 0; |
|
238 } |
|
239 contact->pos[0] = dMUL(radius2,-vDir1[0]) + SpherePos[0]; |
|
240 contact->pos[1] = dMUL(radius2,-vDir1[1]) + SpherePos[1]; |
|
241 contact->pos[2] = dMUL(radius2,-vDir1[2]) + SpherePos[2]; |
|
242 contact->normal[0] = -vDir1[0]; |
|
243 contact->normal[1] = -vDir1[1]; |
|
244 contact->normal[2] = -vDir1[2]; |
|
245 contact->g1 = Cylinder; |
|
246 contact->g2 = Sphere; |
|
247 GeomCount++; |
|
248 return GeomCount; |
|
249 } |
|
250 } |
|
251 } |