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 //****************************************************************************
47 // sphere public API
48
dxSphere(dSpaceID space,dReal _radius)49 dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1)
50 {
51 dAASSERT (_radius >= 0);
52 type = dSphereClass;
53 radius = _radius;
54 updateZeroSizedFlag(!_radius);
55 }
56
57
computeAABB()58 void dxSphere::computeAABB()
59 {
60 aabb[0] = final_posr->pos[0] - radius;
61 aabb[1] = final_posr->pos[0] + radius;
62 aabb[2] = final_posr->pos[1] - radius;
63 aabb[3] = final_posr->pos[1] + radius;
64 aabb[4] = final_posr->pos[2] - radius;
65 aabb[5] = final_posr->pos[2] + radius;
66 }
67
68
dCreateSphere(dSpaceID space,dReal radius)69 dGeomID dCreateSphere (dSpaceID space, dReal radius)
70 {
71 return new dxSphere (space,radius);
72 }
73
74
dGeomSphereSetRadius(dGeomID g,dReal radius)75 void dGeomSphereSetRadius (dGeomID g, dReal radius)
76 {
77 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
78 dAASSERT (radius >= 0);
79 dxSphere *s = (dxSphere*) g;
80 s->radius = radius;
81 s->updateZeroSizedFlag(!radius);
82 dGeomMoved (g);
83 }
84
85
dGeomSphereGetRadius(dGeomID g)86 dReal dGeomSphereGetRadius (dGeomID g)
87 {
88 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
89 dxSphere *s = (dxSphere*) g;
90 return s->radius;
91 }
92
93
dGeomSpherePointDepth(dGeomID g,dReal x,dReal y,dReal z)94 dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
95 {
96 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
97 g->recomputePosr();
98
99 dxSphere *s = (dxSphere*) g;
100 dReal * pos = s->final_posr->pos;
101 return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
102 (y-pos[1])*(y-pos[1]) +
103 (z-pos[2])*(z-pos[2]));
104 }
105
106 //****************************************************************************
107 // pairwise collision functions for standard geom types
108
dCollideSphereSphere(dxGeom * o1,dxGeom * o2,int flags,dContactGeom * contact,int skip)109 int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
110 dContactGeom *contact, int skip)
111 {
112 dIASSERT (skip >= (int)sizeof(dContactGeom));
113 dIASSERT (o1->type == dSphereClass);
114 dIASSERT (o2->type == dSphereClass);
115 dIASSERT ((flags & NUMC_MASK) >= 1);
116
117 dxSphere *sphere1 = (dxSphere*) o1;
118 dxSphere *sphere2 = (dxSphere*) o2;
119
120 contact->g1 = o1;
121 contact->g2 = o2;
122 contact->side1 = -1;
123 contact->side2 = -1;
124
125 return dCollideSpheres (o1->final_posr->pos,sphere1->radius,
126 o2->final_posr->pos,sphere2->radius,contact);
127 }
128
129
dCollideSphereBox(dxGeom * o1,dxGeom * o2,int flags,dContactGeom * contact,int skip)130 int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
131 dContactGeom *contact, int skip)
132 {
133 dIASSERT (skip >= (int)sizeof(dContactGeom));
134 dIASSERT (o1->type == dSphereClass);
135 dIASSERT (o2->type == dBoxClass);
136 dIASSERT ((flags & NUMC_MASK) >= 1);
137
138 // this is easy. get the sphere center `p' relative to the box, and then clip
139 // that to the boundary of the box (call that point `q'). if q is on the
140 // boundary of the box and |p-q| is <= sphere radius, they touch.
141 // if q is inside the box, the sphere is inside the box, so set a contact
142 // normal to push the sphere to the closest box face.
143
144 dVector3 l,t,p,q,r;
145 dReal depth;
146 int onborder = 0;
147
148 dxSphere *sphere = (dxSphere*) o1;
149 dxBox *box = (dxBox*) o2;
150
151 contact->g1 = o1;
152 contact->g2 = o2;
153 contact->side1 = -1;
154 contact->side2 = -1;
155
156 p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
157 p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
158 p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
159
160 l[0] = box->side[0]*REAL(0.5);
161 t[0] = dDOT14(p,o2->final_posr->R);
162 if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
163 if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
164
165 l[1] = box->side[1]*REAL(0.5);
166 t[1] = dDOT14(p,o2->final_posr->R+1);
167 if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
168 if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
169
170 t[2] = dDOT14(p,o2->final_posr->R+2);
171 l[2] = box->side[2]*REAL(0.5);
172 if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
173 if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
174
175 if (!onborder) {
176 // sphere center inside box. find closest face to `t'
177 dReal min_distance = l[0] - dFabs(t[0]);
178 int mini = 0;
179 for (int i=1; i<3; i++) {
180 dReal face_distance = l[i] - dFabs(t[i]);
181 if (face_distance < min_distance) {
182 min_distance = face_distance;
183 mini = i;
184 }
185 }
186 // contact position = sphere center
187 contact->pos[0] = o1->final_posr->pos[0];
188 contact->pos[1] = o1->final_posr->pos[1];
189 contact->pos[2] = o1->final_posr->pos[2];
190 // contact normal points to closest face
191 dVector3 tmp;
192 tmp[0] = 0;
193 tmp[1] = 0;
194 tmp[2] = 0;
195 tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
196 dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp);
197 // contact depth = distance to wall along normal plus radius
198 contact->depth = min_distance + sphere->radius;
199 return 1;
200 }
201
202 t[3] = 0; //@@@ hmmm
203 dMULTIPLY0_331 (q,o2->final_posr->R,t);
204 r[0] = p[0] - q[0];
205 r[1] = p[1] - q[1];
206 r[2] = p[2] - q[2];
207 depth = sphere->radius - dSqrt(dDOT(r,r));
208 if (depth < 0) return 0;
209 contact->pos[0] = q[0] + o2->final_posr->pos[0];
210 contact->pos[1] = q[1] + o2->final_posr->pos[1];
211 contact->pos[2] = q[2] + o2->final_posr->pos[2];
212 contact->normal[0] = r[0];
213 contact->normal[1] = r[1];
214 contact->normal[2] = r[2];
215 dNormalize3 (contact->normal);
216 contact->depth = depth;
217 return 1;
218 }
219
220
dCollideSpherePlane(dxGeom * o1,dxGeom * o2,int flags,dContactGeom * contact,int skip)221 int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
222 dContactGeom *contact, int skip)
223 {
224 dIASSERT (skip >= (int)sizeof(dContactGeom));
225 dIASSERT (o1->type == dSphereClass);
226 dIASSERT (o2->type == dPlaneClass);
227 dIASSERT ((flags & NUMC_MASK) >= 1);
228
229 dxSphere *sphere = (dxSphere*) o1;
230 dxPlane *plane = (dxPlane*) o2;
231
232 contact->g1 = o1;
233 contact->g2 = o2;
234 contact->side1 = -1;
235 contact->side2 = -1;
236
237 dReal k = dDOT (o1->final_posr->pos,plane->p);
238 dReal depth = plane->p[3] - k + sphere->radius;
239 if (depth >= 0) {
240 contact->normal[0] = plane->p[0];
241 contact->normal[1] = plane->p[1];
242 contact->normal[2] = plane->p[2];
243 contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
244 contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
245 contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
246 contact->depth = depth;
247 return 1;
248 }
249 else return 0;
250 }
251