Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/ode/src/collision_cylinder_sphere.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago

[Physik] add ode-0.9
File size: 9.1 KB
Line 
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}
Note: See TracBrowser for help on using the repository browser.