aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp254
1 files changed, 254 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp
new file mode 100644
index 0000000..f85701d
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp
@@ -0,0 +1,254 @@
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 * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de )
26 *
27 * This testing basically comes down to testing the intersection
28 * of the cylinder caps (discs) with the plane.
29 *
30 */
31
32#include <ode/collision.h>
33#include <ode/matrix.h>
34#include <ode/rotation.h>
35#include <ode/odemath.h>
36#include <ode/objects.h>
37
38#include "collision_kernel.h" // for dxGeom
39#include "collision_util.h"
40
41
42int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
43{
44 dIASSERT (skip >= (int)sizeof(dContactGeom));
45 dIASSERT (Cylinder->type == dCylinderClass);
46 dIASSERT (Plane->type == dPlaneClass);
47 dIASSERT ((flags & NUMC_MASK) >= 1);
48
49 int GeomCount = 0; // count of used contactgeoms
50
51#ifdef dSINGLE
52 const dReal toleranz = REAL(0.0001);
53#endif
54#ifdef dDOUBLE
55 const dReal toleranz = REAL(0.0000001);
56#endif
57
58 // Get the properties of the cylinder (length+radius)
59 dReal radius, length;
60 dGeomCylinderGetParams(Cylinder, &radius, &length);
61 dVector3 &cylpos = Cylinder->final_posr->pos;
62 // and the plane
63 dVector4 planevec;
64 dGeomPlaneGetParams(Plane, planevec);
65 dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
66 dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
67
68 dVector3 G1Pos1, G1Pos2, vDir1;
69 vDir1[0] = Cylinder->final_posr->R[2];
70 vDir1[1] = Cylinder->final_posr->R[6];
71 vDir1[2] = Cylinder->final_posr->R[10];
72
73 dReal s;
74 s = length * REAL(0.5);
75 G1Pos2[0] = vDir1[0] * s + cylpos[0];
76 G1Pos2[1] = vDir1[1] * s + cylpos[1];
77 G1Pos2[2] = vDir1[2] * s + cylpos[2];
78
79 G1Pos1[0] = vDir1[0] * -s + cylpos[0];
80 G1Pos1[1] = vDir1[1] * -s + cylpos[1];
81 G1Pos1[2] = vDir1[2] * -s + cylpos[2];
82
83 dVector3 C;
84
85 // parallel-check
86 s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
87 if(s < 0)
88 s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
89 else
90 s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
91 if(s < toleranz && s > (-toleranz))
92 {
93 // discs are parallel to the plane
94
95 // 1.compute if, and where contacts are
96 dVector3 P;
97 s = planevec[3] - dVector3Dot(planevec, G1Pos1);
98 dReal t;
99 t = planevec[3] - dVector3Dot(planevec, G1Pos2);
100 if(s >= t) // s == t does never happen,
101 {
102 if(s >= 0)
103 {
104 // 1. Disc
105 dVector3Copy(G1Pos1, P);
106 }
107 else
108 return GeomCount; // no contacts
109 }
110 else
111 {
112 if(t >= 0)
113 {
114 // 2. Disc
115 dVector3Copy(G1Pos2, P);
116 }
117 else
118 return GeomCount; // no contacts
119 }
120
121 // 2. generate a coordinate-system on the disc
122 dVector3 V1, V2;
123 if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
124 {
125 // not x-axis
126 V1[0] = vDir1[0] + REAL(1.0); // random value
127 V1[1] = vDir1[1];
128 V1[2] = vDir1[2];
129 }
130 else
131 {
132 // maybe x-axis
133 V1[0] = vDir1[0];
134 V1[1] = vDir1[1] + REAL(1.0); // random value
135 V1[2] = vDir1[2];
136 }
137 // V1 is now another direction than vDir1
138 // Cross-product
139 dVector3Cross(V1, vDir1, V2);
140 // make unit V2
141 t = dVector3Length(V2);
142 t = radius / t;
143 dVector3Scale(V2, t);
144 // cross again
145 dVector3Cross(V2, vDir1, V1);
146 // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
147 // V1 = first axis
148 // V2 = second axis
149
150 // 3. generate contactpoints
151
152 // Potential contact 1
153 dVector3Add(P, V1, contact->pos);
154 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
155 if(contact->depth > 0)
156 {
157 dVector3Copy(PlaneNormal, contact->normal);
158 contact->g1 = Cylinder;
159 contact->g2 = Plane;
160 GeomCount++;
161 if( GeomCount >= (flags & NUMC_MASK))
162 return GeomCount; // enough contactgeoms
163 contact = (dContactGeom *)((char *)contact + skip);
164 }
165
166 // Potential contact 2
167 dVector3Subtract(P, V1, contact->pos);
168 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
169 if(contact->depth > 0)
170 {
171 dVector3Copy(PlaneNormal, contact->normal);
172 contact->g1 = Cylinder;
173 contact->g2 = Plane;
174 GeomCount++;
175 if( GeomCount >= (flags & NUMC_MASK))
176 return GeomCount; // enough contactgeoms
177 contact = (dContactGeom *)((char *)contact + skip);
178 }
179
180 // Potential contact 3
181 dVector3Add(P, V2, contact->pos);
182 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
183 if(contact->depth > 0)
184 {
185 dVector3Copy(PlaneNormal, contact->normal);
186 contact->g1 = Cylinder;
187 contact->g2 = Plane;
188 GeomCount++;
189 if( GeomCount >= (flags & NUMC_MASK))
190 return GeomCount; // enough contactgeoms
191 contact = (dContactGeom *)((char *)contact + skip);
192 }
193
194 // Potential contact 4
195 dVector3Subtract(P, V2, contact->pos);
196 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
197 if(contact->depth > 0)
198 {
199 dVector3Copy(PlaneNormal, contact->normal);
200 contact->g1 = Cylinder;
201 contact->g2 = Plane;
202 GeomCount++;
203 if( GeomCount >= (flags & NUMC_MASK))
204 return GeomCount; // enough contactgeoms
205 contact = (dContactGeom *)((char *)contact + skip);
206 }
207 }
208 else
209 {
210 dReal t = dVector3Dot(PlaneNormal, vDir1);
211 C[0] = vDir1[0] * t - PlaneNormal[0];
212 C[1] = vDir1[1] * t - PlaneNormal[1];
213 C[2] = vDir1[2] * t - PlaneNormal[2];
214 s = dVector3Length(C);
215 // move C onto the circle
216 s = radius / s;
217 dVector3Scale(C, s);
218
219 // deepest point of disc 1
220 dVector3Add(C, G1Pos1, contact->pos);
221
222 // depth of the deepest point
223 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
224 if(contact->depth >= 0)
225 {
226 dVector3Copy(PlaneNormal, contact->normal);
227 contact->g1 = Cylinder;
228 contact->g2 = Plane;
229 GeomCount++;
230 if( GeomCount >= (flags & NUMC_MASK))
231 return GeomCount; // enough contactgeoms
232 contact = (dContactGeom *)((char *)contact + skip);
233 }
234
235 // C is still computed
236
237 // deepest point of disc 2
238 dVector3Add(C, G1Pos2, contact->pos);
239
240 // depth of the deepest point
241 contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
242 if(contact->depth >= 0)
243 {
244 dVector3Copy(PlaneNormal, contact->normal);
245 contact->g1 = Cylinder;
246 contact->g2 = Plane;
247 GeomCount++;
248 if( GeomCount >= (flags & NUMC_MASK))
249 return GeomCount; // enough contactgeoms
250 contact = (dContactGeom *)((char *)contact + skip);
251 }
252 }
253 return GeomCount;
254}
generated by cgit v1.1 at 2024-12-25 22:15:29 +0000