Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/src/collision_cylinder_box.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: 28.2 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	* Cylinder-box collider by Alen Ladavac
25	* Ported to ODE by Nguyen Binh
26	*/
27
28	#include <ode/collision.h>
29	#include <ode/matrix.h>
30	#include <ode/rotation.h>
31	#include <ode/odemath.h>
32	#include "collision_util.h"
33
34	static const int MAX_CYLBOX_CLIP_POINTS = 16;
35	static const int nCYLINDER_AXIS = 2;
36	// Number of segment of cylinder base circle.
37	// Must be divisible by 4.
38	static const int nCYLINDER_SEGMENT = 8;
39
40	#define MAX_FLOAT dInfinity
41
42	// Data that passed through the collider's functions
43	typedef struct _sCylinderBoxData
44	{
45	// cylinder parameters
46	dMatrix3 mCylinderRot;
47	dVector3 vCylinderPos;
48	dVector3 vCylinderAxis;
49	dReal fCylinderRadius;
50	dReal fCylinderSize;
51	dVector3 avCylinderNormals[nCYLINDER_SEGMENT];
52
53	// box parameters
54
55	dMatrix3 mBoxRot;
56	dVector3 vBoxPos;
57	dVector3 vBoxHalfSize;
58	// box vertices array : 8 vertices
59	dVector3 avBoxVertices[8];
60
61	// global collider data
62	dVector3 vDiff;
63	dVector3 vNormal;
64	dReal fBestDepth;
65	dReal fBestrb;
66	dReal fBestrc;
67	int iBestAxis;
68
69	// contact data
70	dVector3 vEp0, vEp1;
71	dReal fDepth0, fDepth1;
72
73	// ODE stuff
74	dGeomID gBox;
75	dGeomID gCylinder;
76	dContactGeom* gContact;
77	int iFlags;
78	int iSkip;
79	int nContacts;
80
81	} sCylinderBoxData;
82
83
84	// initialize collision data
85	void _cldInitCylinderBox(sCylinderBoxData& cData)
86	{
87	// get cylinder position, orientation
88	const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
89	dMatrix3Copy(pRotCyc,cData.mCylinderRot);
90
91	const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
92	dVector3Copy(*pPosCyc,cData.vCylinderPos);
93
94	dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
95
96	// get cylinder radius and size
97	dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
98
99	// get box position, orientation, size
100	const dReal* pRotBox = dGeomGetRotation(cData.gBox);
101	dMatrix3Copy(pRotBox,cData.mBoxRot);
102	const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
103	dVector3Copy(*pPosBox,cData.vBoxPos);
104
105	dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
106	cData.vBoxHalfSize[0] *= REAL(0.5);
107	cData.vBoxHalfSize[1] *= REAL(0.5);
108	cData.vBoxHalfSize[2] *= REAL(0.5);
109
110	// vertex 0
111	cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
112	cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
113	cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
114
115	// vertex 1
116	cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
117	cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
118	cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
119
120	// vertex 2
121	cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
122	cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
123	cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
124
125	// vertex 3
126	cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
127	cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
128	cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
129
130	// vertex 4
131	cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
132	cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
133	cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
134
135	// vertex 5
136	cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
137	cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
138	cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
139
140	// vertex 6
141	cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
142	cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
143	cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
144
145	// vertex 7
146	cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
147	cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
148	cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
149
150	// temp index
151	int i = 0;
152	dVector3 vTempBoxVertices[8];
153	// transform vertices in absolute space
154	for(i=0; i < 8; i++)
155	{
156	dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
157	dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
158	}
159
160	// find relative position
161	dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
162	cData.fBestDepth = MAX_FLOAT;
163	cData.vNormal[0] = REAL(0.0);
164	cData.vNormal[1] = REAL(0.0);
165	cData.vNormal[2] = REAL(0.0);
166
167	// calculate basic angle for nCYLINDER_SEGMENT-gon
168	dReal fAngle = M_PI/nCYLINDER_SEGMENT;
169
170	// calculate angle increment
171	dReal fAngleIncrement = fAngle * REAL(2.0);
172
173	// calculate nCYLINDER_SEGMENT-gon points
174	for(i = 0; i < nCYLINDER_SEGMENT; i++)
175	{
176	cData.avCylinderNormals[i][0] = -dCos(fAngle);
177	cData.avCylinderNormals[i][1] = -dSin(fAngle);
178	cData.avCylinderNormals[i][2] = 0;
179
180	fAngle += fAngleIncrement;
181	}
182
183	cData.fBestrb = 0;
184	cData.fBestrc = 0;
185	cData.iBestAxis = 0;
186	cData.nContacts = 0;
187
188	}
189
190	// test for given separating axis
191	int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis )
192	{
193	// check length of input normal
194	dReal fL = dVector3Length(vInputNormal);
195	// if not long enough
196	if ( fL < REAL(1e-5) )
197	{
198	// do nothing
199	return 1;
200	}
201
202	// otherwise make it unit for sure
203	dNormalize3(vInputNormal);
204
205	// project box and Cylinder on mAxis
206	dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);
207
208	dReal frc;
209
210	if (fdot1 > REAL(1.0))
211	{
212	fdot1 = REAL(1.0);
213	frc = dFabs(cData.fCylinderSize*REAL(0.5));
214	}
215
216	// project box and capsule on iAxis
217	frc = dFabs( fdot1 * (cData.fCylinderSizeREAL(0.5))) + cData.fCylinderRadius dSqrt(REAL(1.0)-(fdot1*fdot1));
218
219	dVector3 vTemp1;
220	dReal frb = REAL(0.0);
221
222	dMat3GetCol(cData.mBoxRot,0,vTemp1);
223	frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0];
224
225	dMat3GetCol(cData.mBoxRot,1,vTemp1);
226	frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1];
227
228	dMat3GetCol(cData.mBoxRot,2,vTemp1);
229	frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2];
230
231	// project their distance on separating axis
232	dReal fd = dVector3Dot(cData.vDiff,vInputNormal);
233
234	// if they do not overlap exit, we have no intersection
235	if ( dFabs(fd) > frc+frb )
236	{
237	return 0;
238	}
239
240	// get depth
241	dReal fDepth = - dFabs(fd) + (frc+frb);
242
243	// get maximum depth
244	if ( fDepth < cData.fBestDepth )
245	{
246	cData.fBestDepth = fDepth;
247	dVector3Copy(vInputNormal,cData.vNormal);
248	cData.iBestAxis = iAxis;
249	cData.fBestrb = frb;
250	cData.fBestrc = frc;
251
252	// flip normal if interval is wrong faced
253	if (fd > 0)
254	{
255	dVector3Inv(cData.vNormal);
256	}
257	}
258
259	return 1;
260	}
261
262
263	// check for separation between box edge and cylinder circle edge
264	int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
265	const dVector3 &vCenterPoint,
266	const dVector3 &vVx0, const dVector3 &vVx1,
267	int iAxis )
268	{
269	// calculate direction of edge
270	dVector3 vDirEdge;
271	dVector3Subtract(vVx1,vVx0,vDirEdge);
272	dNormalize3(vDirEdge);
273	// starting point of edge
274	dVector3 vEStart;
275	dVector3Copy(vVx0,vEStart);;
276
277	// calculate angle cosine between cylinder axis and edge
278	dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);
279
280	// if edge is perpendicular to cylinder axis
281	if(dFabs(fdot2) < REAL(1e-5))
282	{
283	// this can't be separating axis, because edge is parallel to circle plane
284	return 1;
285	}
286
287	// find point of intersection between edge line and circle plane
288	dVector3 vTemp1;
289	dVector3Subtract(vCenterPoint,vEStart,vTemp1);
290	dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
291	dVector3 vpnt;
292	vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
293	vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
294	vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);
295
296	// find tangent vector on circle with same center (vCenterPoint) that
297	// touches point of intersection (vpnt)
298	dVector3 vTangent;
299	dVector3Subtract(vCenterPoint,vpnt,vTemp1);
300	dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
301
302	// find vector orthogonal both to tangent and edge direction
303	dVector3 vAxis;
304	dVector3Cross(vTangent,vDirEdge,vAxis);
305
306	// use that vector as separating axis
307	return _cldTestAxis( cData, vAxis, iAxis );
308	}
309
310	// Test separating axis for collision
311	int _cldTestSeparatingAxes(sCylinderBoxData& cData)
312	{
313	// reset best axis
314	cData.fBestDepth = MAX_FLOAT;
315	cData.iBestAxis = 0;
316	cData.fBestrb = 0;
317	cData.fBestrc = 0;
318	cData.nContacts = 0;
319
320	dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
321
322	// Epsilon value for checking axis vector length
323	const dReal fEpsilon = REAL(1e-6);
324
325	// axis A0
326	dMat3GetCol(cData.mBoxRot, 0 , vAxis);
327	if (!_cldTestAxis( cData, vAxis, 1 ))
328	{
329	return 0;
330	}
331
332	// axis A1
333	dMat3GetCol(cData.mBoxRot, 1 , vAxis);
334	if (!_cldTestAxis( cData, vAxis, 2 ))
335	{
336	return 0;
337	}
338
339	// axis A2
340	dMat3GetCol(cData.mBoxRot, 2 , vAxis);
341	if (!_cldTestAxis( cData, vAxis, 3 ))
342	{
343	return 0;
344	}
345
346	// axis C - Cylinder Axis
347	//vAxis = vCylinderAxis;
348	dVector3Copy(cData.vCylinderAxis , vAxis);
349	if (!_cldTestAxis( cData, vAxis, 4 ))
350	{
351	return 0;
352	}
353
354	// axis CxA0
355	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));
356	dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);
357	if(dVector3Length2( vAxis ) > fEpsilon )
358	{
359	if (!_cldTestAxis( cData, vAxis, 5 ))
360	{
361	return 0;
362	}
363	}
364
365	// axis CxA1
366	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));
367	dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);
368	if(dVector3Length2( vAxis ) > fEpsilon )
369	{
370	if (!_cldTestAxis( cData, vAxis, 6 ))
371	{
372	return 0;
373	}
374	}
375
376	// axis CxA2
377	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));
378	dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);
379	if(dVector3Length2( vAxis ) > fEpsilon )
380	{
381	if (!_cldTestAxis( cData, vAxis, 7 ))
382	{
383	return 0;
384	}
385	}
386
387	int i = 0;
388	dVector3 vTemp1;
389	dVector3 vTemp2;
390	// here we check box's vertices axis
391	for(i=0; i< 8; i++)
392	{
393	//vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));
394	dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);
395	dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);
396	//vAxis = ( vCylinderAxis cross vAxis );
397	dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);
398	if(dVector3Length2( vAxis ) > fEpsilon )
399	{
400	if (!_cldTestAxis( cData, vAxis, 8 + i ))
401	{
402	return 0;
403	}
404	}
405	}
406
407	// ************************************
408	// this is defined for first 12 axes
409	// normal of plane that contains top circle of cylinder
410	// center of top circle of cylinder
411	dVector3 vcc;
412	vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
413	vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
414	vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
415	// ************************************
416
417	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16))
418	{
419	return 0;
420	}
421
422	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17))
423	{
424	return 0;
425	}
426
427	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))
428	{
429	return 0;
430	}
431
432	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19))
433	{
434	return 0;
435	}
436
437
438	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))
439	{
440	return 0;
441	}
442
443	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))
444	{
445	return 0;
446	}
447
448	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22))
449	{
450	return 0;
451	}
452
453	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23))
454	{
455	return 0;
456	}
457
458	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24))
459	{
460	return 0;
461	}
462
463	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25))
464	{
465	return 0;
466	}
467
468	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26))
469	{
470	return 0;
471	}
472
473	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27))
474	{
475	return 0;
476	}
477
478	// ************************************
479	// this is defined for second 12 axes
480	// normal of plane that contains bottom circle of cylinder
481	// center of bottom circle of cylinder
482	// vcc = vCylinderPos - vCylinderAxis(fCylinderSizeREAL(0.5));
483	vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
484	vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
485	vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
486	// ************************************
487
488	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28))
489	{
490	return 0;
491	}
492
493	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29))
494	{
495	return 0;
496	}
497
498	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30))
499	{
500	return 0;
501	}
502
503	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31))
504	{
505	return 0;
506	}
507
508	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32))
509	{
510	return 0;
511	}
512
513	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33))
514	{
515	return 0;
516	}
517
518	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34))
519	{
520	return 0;
521	}
522
523	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35))
524	{
525	return 0;
526	}
527
528	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36))
529	{
530	return 0;
531	}
532
533	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37))
534	{
535	return 0;
536	}
537
538	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38))
539	{
540	return 0;
541	}
542
543	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39))
544	{
545	return 0;
546	}
547
548	return 1;
549	}
550
551	int _cldClipCylinderToBox(sCylinderBoxData& cData)
552	{
553	dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK));
554
555	// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
556	dVector3 vN;
557	dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
558	vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1;
559	vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1;
560	vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1;
561
562	// normalize that vector
563	dNormalize3(vN);
564
565	// translate cylinder end points by the vector
566	dVector3 vCposTrans;
567	vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius;
568	vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius;
569	vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius;
570
571	cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
572	cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
573	cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
574
575	cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
576	cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
577	cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
578
579	// transform edge points in box space
580	cData.vEp0[0] -= cData.vBoxPos[0];
581	cData.vEp0[1] -= cData.vBoxPos[1];
582	cData.vEp0[2] -= cData.vBoxPos[2];
583
584	cData.vEp1[0] -= cData.vBoxPos[0];
585	cData.vEp1[1] -= cData.vBoxPos[1];
586	cData.vEp1[2] -= cData.vBoxPos[2];
587
588	dVector3 vTemp1;
589	// clip the edge to box
590	dVector4 plPlane;
591	// plane 0 +x
592	dMat3GetCol(cData.mBoxRot,0,vTemp1);
593	dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
594	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
595	{
596	return 0;
597	}
598
599	// plane 1 +y
600	dMat3GetCol(cData.mBoxRot,1,vTemp1);
601	dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
602	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
603	{
604	return 0;
605	}
606
607	// plane 2 +z
608	dMat3GetCol(cData.mBoxRot,2,vTemp1);
609	dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
610	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
611	{
612	return 0;
613	}
614
615	// plane 3 -x
616	dMat3GetCol(cData.mBoxRot,0,vTemp1);
617	dVector3Inv(vTemp1);
618	dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
619	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
620	{
621	return 0;
622	}
623
624	// plane 4 -y
625	dMat3GetCol(cData.mBoxRot,1,vTemp1);
626	dVector3Inv(vTemp1);
627	dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
628	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
629	{
630	return 0;
631	}
632
633	// plane 5 -z
634	dMat3GetCol(cData.mBoxRot,2,vTemp1);
635	dVector3Inv(vTemp1);
636	dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
637	if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
638	{
639	return 0;
640	}
641
642	// calculate depths for both contact points
643	cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
644	cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
645
646	// clamp depths to 0
647	if(cData.fDepth0<0)
648	{
649	cData.fDepth0 = REAL(0.0);
650	}
651
652	if(cData.fDepth1<0)
653	{
654	cData.fDepth1 = REAL(0.0);
655	}
656
657	// back transform edge points from box to absolute space
658	cData.vEp0[0] += cData.vBoxPos[0];
659	cData.vEp0[1] += cData.vBoxPos[1];
660	cData.vEp0[2] += cData.vBoxPos[2];
661
662	cData.vEp1[0] += cData.vBoxPos[0];
663	cData.vEp1[1] += cData.vBoxPos[1];
664	cData.vEp1[2] += cData.vBoxPos[2];
665
666	dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
667	Contact0->depth = cData.fDepth0;
668	dVector3Copy(cData.vNormal,Contact0->normal);
669	dVector3Copy(cData.vEp0,Contact0->pos);
670	Contact0->g1 = cData.gCylinder;
671	Contact0->g2 = cData.gBox;
672	dVector3Inv(Contact0->normal);
673	cData.nContacts++;
674
675	if (cData.nContacts != (cData.iFlags & NUMC_MASK))
676	{
677	dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
678	Contact1->depth = cData.fDepth1;
679	dVector3Copy(cData.vNormal,Contact1->normal);
680	dVector3Copy(cData.vEp1,Contact1->pos);
681	Contact1->g1 = cData.gCylinder;
682	Contact1->g2 = cData.gBox;
683	dVector3Inv(Contact1->normal);
684	cData.nContacts++;
685	}
686
687	return 1;
688	}
689
690
691	void _cldClipBoxToCylinder(sCylinderBoxData& cData )
692	{
693	dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK));
694
695	dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
696	// check which circle from cylinder we take for clipping
697	if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) )
698	{
699	// get top circle
700	vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
701	vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
702	vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
703
704	vCylinderCircleNormal_Rel[0] = REAL(0.0);
705	vCylinderCircleNormal_Rel[1] = REAL(0.0);
706	vCylinderCircleNormal_Rel[2] = REAL(0.0);
707	vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
708	}
709	else
710	{
711	// get bottom circle
712	vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0](cData.fCylinderSizeREAL(0.5));
713	vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1](cData.fCylinderSizeREAL(0.5));
714	vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2](cData.fCylinderSizeREAL(0.5));
715
716	vCylinderCircleNormal_Rel[0] = REAL(0.0);
717	vCylinderCircleNormal_Rel[1] = REAL(0.0);
718	vCylinderCircleNormal_Rel[2] = REAL(0.0);
719	vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
720	}
721
722	// vNr is normal in Box frame, pointing from Cylinder to Box
723	dVector3 vNr;
724	dMatrix3 mBoxInv;
725
726	// Find a way to use quaternion
727	dMatrix3Inv(cData.mBoxRot,mBoxInv);
728	dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);
729
730	dVector3 vAbsNormal;
731
732	vAbsNormal[0] = dFabs( vNr[0] );
733	vAbsNormal[1] = dFabs( vNr[1] );
734	vAbsNormal[2] = dFabs( vNr[2] );
735
736	// find which face in box is closest to cylinder
737	int iB0, iB1, iB2;
738
739	// Different from Croteam's code
740	if (vAbsNormal[1] > vAbsNormal[0])
741	{
742	// 1 > 0
743	if (vAbsNormal[0]> vAbsNormal[2])
744	{
745	// 0 > 2 -> 1 > 0 >2
746	iB0 = 1; iB1 = 0; iB2 = 2;
747	}
748	else
749	{
750	// 2 > 0-> Must compare 1 and 2
751	if (vAbsNormal[1] > vAbsNormal[2])
752	{
753	// 1 > 2 -> 1 > 2 > 0
754	iB0 = 1; iB1 = 2; iB2 = 0;
755	}
756	else
757	{
758	// 2 > 1 -> 2 > 1 > 0;
759	iB0 = 2; iB1 = 1; iB2 = 0;
760	}
761	}
762	}
763	else
764	{
765	// 0 > 1
766	if (vAbsNormal[1] > vAbsNormal[2])
767	{
768	// 1 > 2 -> 0 > 1 > 2
769	iB0 = 0; iB1 = 1; iB2 = 2;
770	}
771	else
772	{
773	// 2 > 1 -> Must compare 0 and 2
774	if (vAbsNormal[0] > vAbsNormal[2])
775	{
776	// 0 > 2 -> 0 > 2 > 1;
777	iB0 = 0; iB1 = 2; iB2 = 1;
778	}
779	else
780	{
781	// 2 > 0 -> 2 > 0 > 1;
782	iB0 = 2; iB1 = 0; iB2 = 1;
783	}
784	}
785	}
786
787	dVector3 vCenter;
788	// find center of box polygon
789	dVector3 vTemp;
790	if (vNr[iB0] > 0)
791	{
792	dMat3GetCol(cData.mBoxRot,iB0,vTemp);
793	vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
794	vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
795	vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
796	}
797	else
798	{
799	dMat3GetCol(cData.mBoxRot,iB0,vTemp);
800	vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
801	vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
802	vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2];
803	}
804
805	// find the vertices of box polygon
806	dVector3 avPoints[4];
807	dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
808	dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
809
810	int i=0;
811	for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
812	{
813	avTempArray1[i][0] = REAL(0.0);
814	avTempArray1[i][1] = REAL(0.0);
815	avTempArray1[i][2] = REAL(0.0);
816
817	avTempArray2[i][0] = REAL(0.0);
818	avTempArray2[i][1] = REAL(0.0);
819	avTempArray2[i][2] = REAL(0.0);
820	}
821
822	dVector3 vAxis1, vAxis2;
823
824	dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
825	dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
826
827	avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
828	avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
829	avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
830
831	avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
832	avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
833	avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
834
835	avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
836	avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
837	avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
838
839	avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
840	avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
841	avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
842
843	// transform box points to space of cylinder circle
844	dMatrix3 mCylinderInv;
845	dMatrix3Inv(cData.mCylinderRot,mCylinderInv);
846
847	for(i=0; i<4; i++)
848	{
849	dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
850	dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
851	}
852
853	int iTmpCounter1 = 0;
854	int iTmpCounter2 = 0;
855	dVector4 plPlane;
856
857	// plane of cylinder that contains circle for intersection
858	dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
859	dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
860
861
862	// Body of base circle of Cylinder
863	int nCircleSegment = 0;
864	for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
865	{
866	dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
867
868	if (0 == (nCircleSegment % 2))
869	{
870	dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
871	}
872	else
873	{
874	dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
875	}
876
877	dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS );
878	dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
879	}
880
881	// back transform clipped points to absolute space
882	dReal ftmpdot;
883	dReal fTempDepth;
884	dVector3 vPoint;
885
886	if (nCircleSegment % 2)
887	{
888	for( i=0; i<iTmpCounter2; i++)
889	{
890	dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
891	vPoint[0] += vCylinderCirclePos[0];
892	vPoint[1] += vCylinderCirclePos[1];
893	vPoint[2] += vCylinderCirclePos[2];
894
895	dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
896	ftmpdot = dVector3Dot(vTemp, cData.vNormal);
897	fTempDepth = cData.fBestrc - ftmpdot;
898	// Depth must be positive
899	if (fTempDepth > REAL(0.0))
900	{
901	// generate contacts
902	dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
903	Contact0->depth = fTempDepth;
904	dVector3Copy(cData.vNormal,Contact0->normal);
905	dVector3Copy(vPoint,Contact0->pos);
906	Contact0->g1 = cData.gCylinder;
907	Contact0->g2 = cData.gBox;
908	dVector3Inv(Contact0->normal);
909	cData.nContacts++;
910
911	if (cData.nContacts == (cData.iFlags & NUMC_MASK))
912	{
913	break;
914	}
915	}
916	}
917	}
918	else
919	{
920	for( i=0; i<iTmpCounter1; i++)
921	{
922	dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
923	vPoint[0] += vCylinderCirclePos[0];
924	vPoint[1] += vCylinderCirclePos[1];
925	vPoint[2] += vCylinderCirclePos[2];
926
927	dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
928	ftmpdot = dVector3Dot(vTemp, cData.vNormal);
929	fTempDepth = cData.fBestrc - ftmpdot;
930	// Depth must be positive
931	if (fTempDepth > REAL(0.0))
932	{
933	// generate contacts
934	dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
935	Contact0->depth = fTempDepth;
936	dVector3Copy(cData.vNormal,Contact0->normal);
937	dVector3Copy(vPoint,Contact0->pos);
938	Contact0->g1 = cData.gCylinder;
939	Contact0->g2 = cData.gBox;
940	dVector3Inv(Contact0->normal);
941	cData.nContacts++;
942
943	if (cData.nContacts == (cData.iFlags & NUMC_MASK))
944	{
945	break;
946	}
947	}
948	}
949	}
950	}
951
952
953	// Cylinder - Box by CroTeam
954	// Ported by Nguyen Binh
955	int dCollideCylinderBox(dxGeom o1, dxGeom o2, int flags, dContactGeom *contact, int skip)
956	{
957	dIASSERT (skip >= (int)sizeof(dContactGeom));
958	dIASSERT (o1->type == dCylinderClass);
959	dIASSERT (o2->type == dBoxClass);
960	dIASSERT ((flags & NUMC_MASK) >= 1);
961
962	sCylinderBoxData cData;
963
964	// Assign ODE stuff
965	cData.gCylinder = o1;
966	cData.gBox = o2;
967	cData.iFlags = flags;
968	cData.iSkip = skip;
969	cData.gContact = contact;
970
971	// initialize collider
972	_cldInitCylinderBox( cData );
973
974	// do intersection test and find best separating axis
975	if(!_cldTestSeparatingAxes( cData ) )
976	{
977	// if not found do nothing
978	return 0;
979	}
980
981	// if best separation axis is not found
982	if ( cData.iBestAxis == 0 )
983	{
984	// this should not happen (we should already exit in that case)
985	dIASSERT(0);
986	// do nothing
987	return 0;
988	}
989
990	dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis);
991	// choose which clipping method are we going to apply
992	if (dFabs(fdot) < REAL(0.9) )
993	{
994	// clip cylinder over box
995	if(!_cldClipCylinderToBox(cData))
996	{
997	return 0;
998	}
999	}
1000	else
1001	{
1002	_cldClipBoxToCylinder(cData);
1003	}
1004
1005	return cData.nContacts;
1006	}
1007

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

Download in other formats: