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