aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp264
1 files changed, 264 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp
new file mode 100644
index 0000000..964c531
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp
@@ -0,0 +1,264 @@
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 * *
26 * cylinder-sphere collider by Christoph Beyer (boernerb@web.de) *
27 * *
28 * In Cylinder/Sphere-collisions, there are three possibilies: *
29 * 1. collision with the cylinder's nappe *
30 * 2. collision with one of the cylinder's disc *
31 * 3. collision with one of the disc's border *
32 * *
33 * This collider computes two distances (s, t) and based on them, *
34 * it decides, which collision we have. *
35 * This collider always generates 1 (or 0, if we have no collison) *
36 * contacts. *
37 * It is able to "separate" cylinder and sphere in all *
38 * configurations, but it never pays attention to velocity. *
39 * So, in extrem situations, "tunneling-effect" is possible. *
40 * *
41 *******************************************************************/
42
43#include <ode/collision.h>
44#include <ode/matrix.h>
45#include <ode/rotation.h>
46#include <ode/odemath.h>
47#include <ode/objects.h>
48#include "collision_kernel.h" // for dxGeom
49#include "collision_util.h"
50
51int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
52 int flags, dContactGeom *contact, int skip)
53{
54 dIASSERT (skip >= (int)sizeof(dContactGeom));
55 dIASSERT (Cylinder->type == dCylinderClass);
56 dIASSERT (Sphere->type == dSphereClass);
57 dIASSERT ((flags & NUMC_MASK) >= 1);
58
59 unsigned char* pContactData = (unsigned char*)contact;
60 int GeomCount = 0; // count of used contacts
61
62#ifdef dSINGLE
63 const dReal toleranz = REAL(0.0001);
64#endif
65#ifdef dDOUBLE
66 const dReal toleranz = REAL(0.0000001);
67#endif
68
69 // get the data from the geoms
70 dReal radius, length;
71 dGeomCylinderGetParams(Cylinder, &radius, &length);
72 dVector3 &cylpos = Cylinder->final_posr->pos;
73 const dReal* pfRot1 = dGeomGetRotation(Cylinder);
74
75 dReal radius2;
76 radius2 = dGeomSphereGetRadius(Sphere);
77 const dReal* SpherePos = dGeomGetPosition(Sphere);
78
79 // G1Pos1 is the middle of the first disc
80 // G1Pos2 is the middle of the second disc
81 // vDir1 is the unit direction of the cylinderaxis
82 dVector3 G1Pos1, G1Pos2, vDir1;
83 vDir1[0] = Cylinder->final_posr->R[2];
84 vDir1[1] = Cylinder->final_posr->R[6];
85 vDir1[2] = Cylinder->final_posr->R[10];
86
87 dReal s;
88 s = length * REAL(0.5); // just a precomputed factor
89 G1Pos2[0] = vDir1[0] * s + cylpos[0];
90 G1Pos2[1] = vDir1[1] * s + cylpos[1];
91 G1Pos2[2] = vDir1[2] * s + cylpos[2];
92
93 G1Pos1[0] = vDir1[0] * -s + cylpos[0];
94 G1Pos1[1] = vDir1[1] * -s + cylpos[1];
95 G1Pos1[2] = vDir1[2] * -s + cylpos[2];
96
97 dVector3 C;
98 dReal t;
99 // Step 1: compute the two distances 's' and 't'
100 // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
101 s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
102 if(s < (-radius2) || s > (length + radius2) )
103 {
104 // Sphere is too far away from the discs
105 // no collision
106 return 0;
107 }
108
109 // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
110 C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
111 C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
112 C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
113 // t is the distance from the Sphere-middle to the cylinder-axis!
114 t = dVector3Length(C);
115 if(t > (radius + radius2) )
116 {
117 // Sphere is too far away from the cylinder axis!
118 // no collision
119 return 0;
120 }
121
122 // decide which kind of collision we have:
123 if(t > radius && (s < 0 || s > length) )
124 {
125 // 3. collision
126 if(s <= 0)
127 {
128 contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
129 if(contact->depth < 0)
130 {
131 // no collision!
132 return 0;
133 }
134 contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
135 contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
136 contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
137 contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
138 contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
139 contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
140 contact->g1 = Cylinder;
141 contact->g2 = Sphere;
142 GeomCount++;
143 return GeomCount;
144 }
145 else
146 {
147 // now s is bigger than length here!
148 contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
149 if(contact->depth < 0)
150 {
151 // no collision!
152 return 0;
153 }
154 contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
155 contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
156 contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
157 contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
158 contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
159 contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
160 contact->g1 = Cylinder;
161 contact->g2 = Sphere;
162 GeomCount++;
163 return GeomCount;
164 }
165 }
166 else if( (radius - t) <= s && (radius - t) <= (length - s) )
167 {
168 // 1. collsision
169 if(t > (radius2 + toleranz))
170 {
171 // cylinder-axis is outside the sphere
172 contact->depth = (radius2 + radius) - t;
173 if(contact->depth < 0)
174 {
175 // should never happen, but just for safeness
176 return 0;
177 }
178 else
179 {
180 C[0] /= t;
181 C[1] /= t;
182 C[2] /= t;
183 contact->pos[0] = C[0] * radius2 + SpherePos[0];
184 contact->pos[1] = C[1] * radius2 + SpherePos[1];
185 contact->pos[2] = C[2] * radius2 + SpherePos[2];
186 contact->normal[0] = C[0];
187 contact->normal[1] = C[1];
188 contact->normal[2] = C[2];
189 contact->g1 = Cylinder;
190 contact->g2 = Sphere;
191 GeomCount++;
192 return GeomCount;
193 }
194 }
195 else
196 {
197 // cylinder-axis is outside of the sphere
198 contact->depth = (radius2 + radius) - t;
199 if(contact->depth < 0)
200 {
201 // should never happen, but just for safeness
202 return 0;
203 }
204 else
205 {
206 contact->pos[0] = C[0] + SpherePos[0];
207 contact->pos[1] = C[1] + SpherePos[1];
208 contact->pos[2] = C[2] + SpherePos[2];
209 contact->normal[0] = C[0] / t;
210 contact->normal[1] = C[1] / t;
211 contact->normal[2] = C[2] / t;
212 contact->g1 = Cylinder;
213 contact->g2 = Sphere;
214 GeomCount++;
215 return GeomCount;
216 }
217 }
218 }
219 else
220 {
221 // 2. collision
222 if(s <= (length * REAL(0.5)) )
223 {
224 // collsision with the first disc
225 contact->depth = s + radius2;
226 if(contact->depth < 0)
227 {
228 // should never happen, but just for safeness
229 return 0;
230 }
231 contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
232 contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
233 contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
234 contact->normal[0] = vDir1[0];
235 contact->normal[1] = vDir1[1];
236 contact->normal[2] = vDir1[2];
237 contact->g1 = Cylinder;
238 contact->g2 = Sphere;
239 GeomCount++;
240 return GeomCount;
241 }
242 else
243 {
244 // collsision with the second disc
245 contact->depth = (radius2 + length - s);
246 if(contact->depth < 0)
247 {
248 // should never happen, but just for safeness
249 return 0;
250 }
251 contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
252 contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
253 contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
254 contact->normal[0] = -vDir1[0];
255 contact->normal[1] = -vDir1[1];
256 contact->normal[2] = -vDir1[2];
257 contact->g1 = Cylinder;
258 contact->g2 = Sphere;
259 GeomCount++;
260 return GeomCount;
261 }
262 }
263 return GeomCount;
264}
generated by cgit v1.1 at 2024-12-26 11:43:27 +0000