aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/capsule.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:07 +0000
committerdan miller2007-10-19 05:20:07 +0000
commitfca74b0bf0a0833f5701e9c0de7b3bc15b2233dd (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/ode/src/capsule.cpp
parentresubmitting ode (diff)
downloadopensim-SC_OLD-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.zip
opensim-SC_OLD-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.gz
opensim-SC_OLD-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.bz2
opensim-SC_OLD-fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd.tar.xz
dont ask
Diffstat (limited to 'libraries/ode-0.9/ode/src/capsule.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/capsule.cpp369
1 files changed, 0 insertions, 369 deletions
diff --git a/libraries/ode-0.9/ode/src/capsule.cpp b/libraries/ode-0.9/ode/src/capsule.cpp
deleted file mode 100644
index 5680baa..0000000
--- a/libraries/ode-0.9/ode/src/capsule.cpp
+++ /dev/null
@@ -1,369 +0,0 @@
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
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::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
48dxCapsule::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
58void 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
75dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length)
76{
77 return new dxCapsule (space,radius,length);
78}
79
80
81void 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
92void 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
101dReal 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
127int 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
159int 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
198int 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
312int 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
generated by cgit v1.1 at 2024-11-11 03:10:50 +0000