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