Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/src/collision_cylinder_plane.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: 7.6 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	* 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
42	int 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	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: