|
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 standard ODE geometry primitives: public API and pairwise collision functions. |
|
26 |
|
27 the rule is that only the low level primitive collision functions should set |
|
28 dContactGeom::g1 and dContactGeom::g2. |
|
29 |
|
30 */ |
|
31 |
|
32 #include <ode/common.h> |
|
33 #include <ode/collision.h> |
|
34 #include <ode/matrix.h> |
|
35 #include <ode/rotation.h> |
|
36 #include <ode/odemath.h> |
|
37 #include "collision_kernel.h" |
|
38 #include "collision_std.h" |
|
39 #include "collision_util.h" |
|
40 |
|
41 //**************************************************************************** |
|
42 // capped cylinder public API |
|
43 |
|
44 dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) : |
|
45 dxGeom (space,1) |
|
46 { |
|
47 type = dCapsuleClass; |
|
48 radius = _radius; |
|
49 lz = _length; |
|
50 } |
|
51 |
|
52 |
|
53 void dxCapsule::computeAABB() |
|
54 { |
|
55 const dMatrix3& R = final_posr->R; |
|
56 const dVector3& pos = final_posr->pos; |
|
57 |
|
58 dReal xrange = dMUL(dFabs(dMUL(R[2],lz)),REAL(0.5)) + radius; |
|
59 dReal yrange = dMUL(dFabs(dMUL(R[6],lz)),REAL(0.5)) + radius; |
|
60 dReal zrange = dMUL(dFabs(dMUL(R[10],lz)),REAL(0.5)) + radius; |
|
61 aabb[0] = pos[0] - xrange; |
|
62 aabb[1] = pos[0] + xrange; |
|
63 aabb[2] = pos[1] - yrange; |
|
64 aabb[3] = pos[1] + yrange; |
|
65 aabb[4] = pos[2] - zrange; |
|
66 aabb[5] = pos[2] + zrange; |
|
67 } |
|
68 |
|
69 |
|
70 EXPORT_C dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length) |
|
71 { |
|
72 return new dxCapsule (space,radius,length); |
|
73 } |
|
74 |
|
75 |
|
76 EXPORT_C void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) |
|
77 { |
|
78 dxCapsule *c = (dxCapsule*) g; |
|
79 c->radius = radius; |
|
80 c->lz = length; |
|
81 dGeomMoved (g); |
|
82 } |
|
83 |
|
84 |
|
85 EXPORT_C void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length) |
|
86 { |
|
87 dxCapsule *c = (dxCapsule*) g; |
|
88 *radius = c->radius; |
|
89 *length = c->lz; |
|
90 } |
|
91 |
|
92 |
|
93 EXPORT_C dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z) |
|
94 { |
|
95 g->recomputePosr(); |
|
96 dxCapsule *c = (dxCapsule*) g; |
|
97 |
|
98 const dReal* R = g->final_posr->R; |
|
99 const dReal* pos = g->final_posr->pos; |
|
100 |
|
101 dVector3 a; |
|
102 a[0] = x - pos[0]; |
|
103 a[1] = y - pos[1]; |
|
104 a[2] = z - pos[2]; |
|
105 dReal beta = dDOT14(a,R+2); |
|
106 dReal lz2 = dMUL(c->lz,REAL(0.5)); |
|
107 if (beta < -lz2) beta = -lz2; |
|
108 else if (beta > lz2) beta = lz2; |
|
109 a[0] = c->final_posr->pos[0] + dMUL(beta,R[0*4+2]); |
|
110 a[1] = c->final_posr->pos[1] + dMUL(beta,R[1*4+2]); |
|
111 a[2] = c->final_posr->pos[2] + dMUL(beta,R[2*4+2]); |
|
112 return c->radius - |
|
113 dSqrt (dMUL((x-a[0]),(x-a[0])) + dMUL((y-a[1]),(y-a[1])) + dMUL((z-a[2]),(z-a[2]))); |
|
114 } |
|
115 |
|
116 |
|
117 |
|
118 int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int /*flags*/, |
|
119 dContactGeom *contact, int /*skip*/) |
|
120 { |
|
121 dxCapsule *ccyl = (dxCapsule*) o1; |
|
122 dxSphere *sphere = (dxSphere*) o2; |
|
123 |
|
124 contact->g1 = o1; |
|
125 contact->g2 = o2; |
|
126 |
|
127 // find the point on the cylinder axis that is closest to the sphere |
|
128 dReal alpha = |
|
129 dMUL(o1->final_posr->R[2],(o2->final_posr->pos[0] - o1->final_posr->pos[0])) + |
|
130 dMUL(o1->final_posr->R[6],(o2->final_posr->pos[1] - o1->final_posr->pos[1])) + |
|
131 dMUL(o1->final_posr->R[10],(o2->final_posr->pos[2] - o1->final_posr->pos[2])); |
|
132 dReal lz2 = dMUL(ccyl->lz,REAL(0.5)); |
|
133 if (alpha > lz2) alpha = lz2; |
|
134 if (alpha < -lz2) alpha = -lz2; |
|
135 |
|
136 // collide the spheres |
|
137 dVector3 p; |
|
138 p[0] = o1->final_posr->pos[0] + dMUL(alpha,o1->final_posr->R[2]); |
|
139 p[1] = o1->final_posr->pos[1] + dMUL(alpha,o1->final_posr->R[6]); |
|
140 p[2] = o1->final_posr->pos[2] + dMUL(alpha,o1->final_posr->R[10]); |
|
141 return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact); |
|
142 } |
|
143 |
|
144 |
|
145 int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int /*flags*/, |
|
146 dContactGeom *contact, int /*skip*/) |
|
147 { |
|
148 dxCapsule *cyl = (dxCapsule*) o1; |
|
149 dxBox *box = (dxBox*) o2; |
|
150 |
|
151 contact->g1 = o1; |
|
152 contact->g2 = o2; |
|
153 |
|
154 // get p1,p2 = cylinder axis endpoints, get radius |
|
155 dVector3 p1,p2; |
|
156 dReal clen = dMUL(cyl->lz,REAL(0.5)); |
|
157 p1[0] = o1->final_posr->pos[0] + dMUL(clen,o1->final_posr->R[2]); |
|
158 p1[1] = o1->final_posr->pos[1] + dMUL(clen,o1->final_posr->R[6]); |
|
159 p1[2] = o1->final_posr->pos[2] + dMUL(clen,o1->final_posr->R[10]); |
|
160 p2[0] = o1->final_posr->pos[0] - dMUL(clen,o1->final_posr->R[2]); |
|
161 p2[1] = o1->final_posr->pos[1] - dMUL(clen,o1->final_posr->R[6]); |
|
162 p2[2] = o1->final_posr->pos[2] - dMUL(clen,o1->final_posr->R[10]); |
|
163 dReal radius = cyl->radius; |
|
164 |
|
165 // copy out box center, rotation matrix, and side array |
|
166 dReal *c = o2->final_posr->pos; |
|
167 dReal *R = o2->final_posr->R; |
|
168 const dReal *side = box->side; |
|
169 |
|
170 // get the closest point between the cylinder axis and the box |
|
171 dVector3 pl,pb; |
|
172 dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); |
|
173 |
|
174 // generate contact point |
|
175 return dCollideSpheres (pl,radius,pb,0,contact); |
|
176 } |
|
177 |
|
178 |
|
179 int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, |
|
180 int flags, dContactGeom *contact, int skip) |
|
181 { |
|
182 int i; |
|
183 const dReal tolerance = REAL(2e-5); |
|
184 |
|
185 dxCapsule *cyl1 = (dxCapsule*) o1; |
|
186 dxCapsule *cyl2 = (dxCapsule*) o2; |
|
187 |
|
188 contact->g1 = o1; |
|
189 contact->g2 = o2; |
|
190 |
|
191 // copy out some variables, for convenience |
|
192 dReal lz1 = dMUL(cyl1->lz,REAL(0.5)); |
|
193 dReal lz2 = dMUL(cyl2->lz,REAL(0.5)); |
|
194 dReal *pos1 = o1->final_posr->pos; |
|
195 dReal *pos2 = o2->final_posr->pos; |
|
196 dReal axis1[3],axis2[3]; |
|
197 axis1[0] = o1->final_posr->R[2]; |
|
198 axis1[1] = o1->final_posr->R[6]; |
|
199 axis1[2] = o1->final_posr->R[10]; |
|
200 axis2[0] = o2->final_posr->R[2]; |
|
201 axis2[1] = o2->final_posr->R[6]; |
|
202 axis2[2] = o2->final_posr->R[10]; |
|
203 |
|
204 // if the cylinder axes are close to parallel, we'll try to detect up to |
|
205 // two contact points along the body of the cylinder. if we can't find any |
|
206 // points then we'll fall back to the closest-points algorithm. note that |
|
207 // we are not treating this special case for reasons of degeneracy, but |
|
208 // because we want two contact points in some situations. the closet-points |
|
209 // algorithm is robust in all casts, but it can return only one contact. |
|
210 |
|
211 dVector3 sphere1,sphere2; |
|
212 dReal a1a2 = dDOT (axis1,axis2); |
|
213 dReal det = REAL(1.0)-dMUL(a1a2,a1a2); |
|
214 if (det < tolerance) { |
|
215 // the cylinder axes (almost) parallel, so we will generate up to two |
|
216 // contacts. alpha1 and alpha2 (line position parameters) are related by: |
|
217 // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) |
|
218 // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) |
|
219 // first compute where the two cylinders overlap in alpha1 space: |
|
220 if (a1a2 < 0) { |
|
221 axis2[0] = -axis2[0]; |
|
222 axis2[1] = -axis2[1]; |
|
223 axis2[2] = -axis2[2]; |
|
224 } |
|
225 dReal q[3]; |
|
226 for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; |
|
227 dReal k = dDOT (axis1,q); |
|
228 dReal a1lo = -lz1; |
|
229 dReal a1hi = lz1; |
|
230 dReal a2lo = -lz2 - k; |
|
231 dReal a2hi = lz2 - k; |
|
232 dReal lo = (a1lo > a2lo) ? a1lo : a2lo; |
|
233 dReal hi = (a1hi < a2hi) ? a1hi : a2hi; |
|
234 if (lo <= hi) { |
|
235 int num_contacts = flags & NUMC_MASK; |
|
236 if (num_contacts >= 2 && lo < hi) { |
|
237 // generate up to two contacts. if one of those contacts is |
|
238 // not made, fall back on the one-contact strategy. |
|
239 for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(lo,axis1[i]); |
|
240 for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL((lo+k),axis2[i]); |
|
241 int n1 = dCollideSpheres (sphere1,cyl1->radius, |
|
242 sphere2,cyl2->radius,contact); |
|
243 if (n1) { |
|
244 for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(hi,axis1[i]); |
|
245 for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL((hi+k),axis2[i]); |
|
246 dContactGeom *c2 = CONTACT(contact,skip); |
|
247 int n2 = dCollideSpheres (sphere1,cyl1->radius, |
|
248 sphere2,cyl2->radius, c2); |
|
249 if (n2) { |
|
250 c2->g1 = o1; |
|
251 c2->g2 = o2; |
|
252 return 2; |
|
253 } |
|
254 } |
|
255 } |
|
256 |
|
257 // just one contact to generate, so put it in the middle of |
|
258 // the range |
|
259 dReal alpha1 = dMUL((lo + hi),REAL(0.5)); |
|
260 dReal alpha2 = alpha1 + k; |
|
261 for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(alpha1,axis1[i]); |
|
262 for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL(alpha2,axis2[i]); |
|
263 return dCollideSpheres (sphere1,cyl1->radius, |
|
264 sphere2,cyl2->radius,contact); |
|
265 } |
|
266 } |
|
267 |
|
268 // use the closest point algorithm |
|
269 dVector3 a1,a2,b1,b2; |
|
270 a1[0] = o1->final_posr->pos[0] + dMUL(axis1[0],lz1); |
|
271 a1[1] = o1->final_posr->pos[1] + dMUL(axis1[1],lz1); |
|
272 a1[2] = o1->final_posr->pos[2] + dMUL(axis1[2],lz1); |
|
273 a2[0] = o1->final_posr->pos[0] - dMUL(axis1[0],lz1); |
|
274 a2[1] = o1->final_posr->pos[1] - dMUL(axis1[1],lz1); |
|
275 a2[2] = o1->final_posr->pos[2] - dMUL(axis1[2],lz1); |
|
276 b1[0] = o2->final_posr->pos[0] + dMUL(axis2[0],lz2); |
|
277 b1[1] = o2->final_posr->pos[1] + dMUL(axis2[1],lz2); |
|
278 b1[2] = o2->final_posr->pos[2] + dMUL(axis2[2],lz2); |
|
279 b2[0] = o2->final_posr->pos[0] - dMUL(axis2[0],lz2); |
|
280 b2[1] = o2->final_posr->pos[1] - dMUL(axis2[1],lz2); |
|
281 b2[2] = o2->final_posr->pos[2] - dMUL(axis2[2],lz2); |
|
282 |
|
283 dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2); |
|
284 return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); |
|
285 } |
|
286 |
|
287 |
|
288 int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags, |
|
289 dContactGeom *contact, int skip) |
|
290 { |
|
291 dxCapsule *ccyl = (dxCapsule*) o1; |
|
292 dxPlane *plane = (dxPlane*) o2; |
|
293 |
|
294 // collide the deepest capping sphere with the plane |
|
295 dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0); |
|
296 dVector3 p; |
|
297 p[0] = o1->final_posr->pos[0] + dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
298 p[1] = o1->final_posr->pos[1] + dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
299 p[2] = o1->final_posr->pos[2] + dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
300 |
|
301 dReal k = dDOT (p,plane->p); |
|
302 dReal depth = plane->p[3] - k + ccyl->radius; |
|
303 if (depth < 0) return 0; |
|
304 contact->normal[0] = plane->p[0]; |
|
305 contact->normal[1] = plane->p[1]; |
|
306 contact->normal[2] = plane->p[2]; |
|
307 contact->pos[0] = p[0] - dMUL(plane->p[0],ccyl->radius); |
|
308 contact->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius); |
|
309 contact->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius); |
|
310 contact->depth = depth; |
|
311 |
|
312 int ncontacts = 1; |
|
313 if ((flags & NUMC_MASK) >= 2) { |
|
314 // collide the other capping sphere with the plane |
|
315 p[0] = o1->final_posr->pos[0] - dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
316 p[1] = o1->final_posr->pos[1] - dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
317 p[2] = o1->final_posr->pos[2] - dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign))); |
|
318 |
|
319 k = dDOT (p,plane->p); |
|
320 depth = plane->p[3] - k + ccyl->radius; |
|
321 if (depth >= 0) { |
|
322 dContactGeom *c2 = CONTACT(contact,skip); |
|
323 c2->normal[0] = plane->p[0]; |
|
324 c2->normal[1] = plane->p[1]; |
|
325 c2->normal[2] = plane->p[2]; |
|
326 c2->pos[0] = p[0] - dMUL(plane->p[0],ccyl->radius); |
|
327 c2->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius); |
|
328 c2->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius); |
|
329 c2->depth = depth; |
|
330 ncontacts = 2; |
|
331 } |
|
332 } |
|
333 |
|
334 for (int i=0; i < ncontacts; i++) { |
|
335 CONTACT(contact,i*skip)->g1 = o1; |
|
336 CONTACT(contact,i*skip)->g2 = o2; |
|
337 } |
|
338 return ncontacts; |
|
339 } |
|
340 |