Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/src/collision_trimesh_ccylinder.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: 30.9 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	* Triangle-Capsule(Capsule) collider by Alen Ladavac
25	* Ported to ODE by Nguyen Binh
26	*/
27
28	// NOTES from Nguyen Binh
29	// 14 Apr : Seem to be robust
30	// There is a problem when you use original Step and set contact friction
31	// surface.mu = dInfinity;
32	// More description :
33	// When I dropped Capsule over the bunny ears, it seems to stuck
34	// there for a while. I think the cause is when you set surface.mu = dInfinity;
35	// the friction force is too high so it just hang the capsule there.
36	// So the good cure for this is to set mu = around 1.5 (in my case)
37	// For StepFast1, this become as solid as rock : StepFast1 just approximate
38	// friction force.
39
40	// NOTES from Croteam's Alen
41	//As a side note... there are some extra contacts that can be generated
42	//on the edge between two triangles, and if the capsule penetrates deeply into
43	//the triangle (usually happens with large mass or low FPS), some such
44	//contacts can in some cases push the capsule away from the edge instead of
45	//away from the two triangles. This shows up as capsule slowing down a bit
46	//when hitting an edge while sliding along a flat tesselated grid of
47	//triangles. This is only if capsule is standing upwards.
48
49	//Same thing can appear whenever a smooth object (e.g sphere) hits such an
50	//edge, and it needs to be solved as a special case probably. This is a
51	//problem we are looking forward to address soon.
52
53	#include <ode/collision.h>
54	#include <ode/matrix.h>
55	#include <ode/rotation.h>
56	#include <ode/odemath.h>
57	#include "collision_util.h"
58
59	#define TRIMESH_INTERNAL
60	#include "collision_trimesh_internal.h"
61
62	#if dTRIMESH_ENABLED
63
64	// OPCODE version
65	#if dTRIMESH_OPCODE
66	// largest number, double or float
67	#if defined(dSINGLE)
68	#define MAX_REAL FLT_MAX
69	#define MIN_REAL (-FLT_MAX)
70	#else
71	#define MAX_REAL DBL_MAX
72	#define MIN_REAL (-DBL_MAX)
73	#endif
74
75	// To optimize before send contacts to dynamic part
76	#define OPTIMIZE_CONTACTS
77
78	// dVector3
79	// r=a-b
80	#define SUBTRACT(a,b,r) \
81	(r)[0]=(a)[0] - (b)[0]; \
82	(r)[1]=(a)[1] - (b)[1]; \
83	(r)[2]=(a)[2] - (b)[2];
84
85
86	// dVector3
87	// a=b
88	#define SET(a,b) \
89	(a)[0]=(b)[0]; \
90	(a)[1]=(b)[1]; \
91	(a)[2]=(b)[2];
92
93
94	// dMatrix3
95	// a=b
96	#define SETM(a,b) \
97	(a)[0]=(b)[0]; \
98	(a)[1]=(b)[1]; \
99	(a)[2]=(b)[2]; \
100	(a)[3]=(b)[3]; \
101	(a)[4]=(b)[4]; \
102	(a)[5]=(b)[5]; \
103	(a)[6]=(b)[6]; \
104	(a)[7]=(b)[7]; \
105	(a)[8]=(b)[8]; \
106	(a)[9]=(b)[9]; \
107	(a)[10]=(b)[10]; \
108	(a)[11]=(b)[11];
109
110
111	// dVector3
112	// r=a+b
113	#define ADD(a,b,r) \
114	(r)[0]=(a)[0] + (b)[0]; \
115	(r)[1]=(a)[1] + (b)[1]; \
116	(r)[2]=(a)[2] + (b)[2];
117
118
119	// dMatrix3, int, dVector3
120	// v=column a from m
121	#define GETCOL(m,a,v) \
122	(v)[0]=(m)[(a)+0]; \
123	(v)[1]=(m)[(a)+4]; \
124	(v)[2]=(m)[(a)+8];
125
126
127	// dVector4, dVector3
128	// distance between plane p and point v
129	#define POINTDISTANCE(p,v) \
130	( p[0]v[0] + p[1]v[1] + p[2]*v[2] + p[3] ); \
131
132
133	// dVector4, dVector3, dReal
134	// construct plane from normal and d
135	#define CONSTRUCTPLANE(plane,normal,d) \
136	plane[0]=normal[0];\
137	plane[1]=normal[1];\
138	plane[2]=normal[2];\
139	plane[3]=d;
140
141
142	// dVector3
143	// length of vector a
144	#define LENGTHOF(a) \
145	dSqrt(a[0]a[0]+a[1]a[1]+a[2]*a[2]);\
146
147	inline dReal _length2OfVector3(dVector3 v)
148	{
149	return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] );
150	}
151
152
153	// Local contacts data
154	typedef struct _sLocalContactData
155	{
156	dVector3 vPos;
157	dVector3 vNormal;
158	dReal fDepth;
159	int triIndex;
160	int nFlags; // 0 = filtered out, 1 = OK
161	}sLocalContactData;
162
163	static sLocalContactData *gLocalContacts;
164	static unsigned int ctContacts = 0;
165
166	// capsule data
167	// real time data
168	static dMatrix3 mCapsuleRotation;
169	static dVector3 vCapsulePosition;
170	static dVector3 vCapsuleAxis;
171	// static data
172	static dReal vCapsuleRadius;
173	static dReal fCapsuleSize;
174
175	// mesh data
176	static dMatrix4 mHullDstPl;
177	static dMatrix3 mTriMeshRot;
178	static dVector3 mTriMeshPos;
179	static dVector3 vE0, vE1, vE2;
180
181	// Two geom
182	dxGeom* gCylinder;
183	dxGeom* gTriMesh;
184
185	// global collider data
186	static dVector3 vNormal;
187	static dReal fBestDepth;
188	static dReal fBestCenter;
189	static dReal fBestrt;
190	static int iBestAxis;
191	static dVector3 vN = {0,0,0,0};
192
193	static dVector3 vV0;
194	static dVector3 vV1;
195	static dVector3 vV2;
196
197	// ODE contact's specific
198	static unsigned int iFlags;
199	static dContactGeom *ContactGeoms;
200	static int iStride;
201
202	// Capsule lie on axis number 3 = (Z axis)
203	static const int nCAPSULE_AXIS = 2;
204
205	// Use to classify contacts to be "near" in position
206	static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
207	// Use to classify contacts to be "near" in normal direction
208	static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
209
210
211	// If this two contact can be classified as "near"
212	inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
213	{
214	int bPosNear = 0;
215	int bSameDir = 0;
216	dVector3 vDiff;
217
218	// First check if they are "near" in position
219	SUBTRACT(c1.vPos,c2.vPos,vDiff);
220	if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
221	&&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
222	&&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
223	{
224	bPosNear = 1;
225	}
226
227	// Second check if they are "near" in normal direction
228	SUBTRACT(c1.vNormal,c2.vNormal,vDiff);
229	if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
230	&&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
231	&&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
232	{
233	bSameDir = 1;
234	}
235
236	// Will be "near" if position and normal direction are "near"
237	return (bPosNear && bSameDir);
238	}
239
240	inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
241	{
242	// The not better will be throw away
243	// You can change the selection criteria here
244	return (c1.fDepth > c2.fDepth);
245	}
246
247	// iterate through gLocalContacts and filtered out "near contact"
248	inline void _OptimizeLocalContacts()
249	{
250	int nContacts = ctContacts;
251
252	for (int i = 0; i < nContacts-1; i++)
253	{
254	for (int j = i+1; j < nContacts; j++)
255	{
256	if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j]))
257	{
258	// If they are seem to be the samed then filtered
259	// out the least penetrate one
260	if (_IsBetter(gLocalContacts[j],gLocalContacts[i]))
261	{
262	gLocalContacts[i].nFlags = 0; // filtered 1st contact
263	}
264	else
265	{
266	gLocalContacts[j].nFlags = 0; // filtered 2nd contact
267	}
268
269	// NOTE
270	// There is other way is to add two depth together but
271	// it not work so well. Why???
272	}
273	}
274	}
275	}
276
277	inline int _ProcessLocalContacts()
278	{
279	if (ctContacts == 0)
280	{
281	return 0;
282	}
283
284	#ifdef OPTIMIZE_CONTACTS
285	if (ctContacts > 1 && !(iFlags & CONTACTS_UNIMPORTANT))
286	{
287	// Can be optimized...
288	_OptimizeLocalContacts();
289	}
290	#endif
291
292	unsigned int iContact = 0;
293	dContactGeom* Contact = 0;
294
295	unsigned int nFinalContact = 0;
296
297	for (iContact = 0; iContact < ctContacts; iContact ++)
298	{
299	// Ensure that we haven't created too many contacts
300	if( nFinalContact >= (iFlags & NUMC_MASK))
301	{
302	break;
303	}
304
305	if (1 == gLocalContacts[iContact].nFlags)
306	{
307	Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride);
308	Contact->depth = gLocalContacts[iContact].fDepth;
309	SET(Contact->normal,gLocalContacts[iContact].vNormal);
310	SET(Contact->pos,gLocalContacts[iContact].vPos);
311	Contact->g1 = gTriMesh;
312	Contact->g2 = gCylinder;
313	Contact->side2 = gLocalContacts[iContact].triIndex;
314
315	nFinalContact++;
316	}
317	}
318	// debug
319	//if (nFinalContact != ctContacts)
320	//{
321	// printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact);
322	//}
323
324	return nFinalContact;
325	}
326
327	BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
328	{
329	// calculate distance of edge points to plane
330	dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 );
331	dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 );
332
333	// if both points are behind the plane
334	if ( fDistance0 < 0 && fDistance1 < 0 )
335	{
336	// do nothing
337	return FALSE;
338	// if both points in front of the plane
339	} else if ( fDistance0 > 0 && fDistance1 > 0 )
340	{
341	// accept them
342	return TRUE;
343	// if we have edge/plane intersection
344	} else if ((fDistance0 > 0 && fDistance1 < 0) \|\| ( fDistance0 < 0 && fDistance1 > 0))
345	{
346
347	// find intersection point of edge and plane
348	dVector3 vIntersectionPoint;
349	vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
350	vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
351	vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
352
353	// clamp correct edge to intersection point
354	if ( fDistance0 < 0 )
355	{
356	SET(vEpnt0,vIntersectionPoint);
357	} else
358	{
359	SET(vEpnt1,vIntersectionPoint);
360	}
361	return TRUE;
362	}
363	return TRUE;
364	}
365
366	static BOOL _cldTestAxis(const dVector3 &v0,
367	const dVector3 &v1,
368	const dVector3 &v2,
369	dVector3 vAxis,
370	int iAxis,
371	BOOL bNoFlip = FALSE)
372	{
373
374	// calculate length of separating axis vector
375	dReal fL = LENGTHOF(vAxis);
376	// if not long enough
377	// TODO : dReal epsilon please
378	if ( fL < REAL(1e-5) )
379	{
380	// do nothing
381	//iLastOutAxis = 0;
382	return TRUE;
383	}
384
385	// otherwise normalize it
386	dNormalize3(vAxis);
387
388	// project capsule on vAxis
389	dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))(fCapsuleSizeREAL(0.5)-vCapsuleRadius) + vCapsuleRadius;
390
391	// project triangle on vAxis
392	dReal afv[3];
393	afv[0] = dDOT( vV0 , vAxis );
394	afv[1] = dDOT( vV1 , vAxis );
395	afv[2] = dDOT( vV2 , vAxis );
396
397	dReal fMin = MAX_REAL;
398	dReal fMax = MIN_REAL;
399
400	// for each vertex
401	for(int i=0; i<3; i++)
402	{
403	// find minimum
404	if (afv[i]<fMin)
405	{
406	fMin = afv[i];
407	}
408	// find maximum
409	if (afv[i]>fMax)
410	{
411	fMax = afv[i];
412	}
413	}
414
415	// find triangle's center of interval on axis
416	dReal fCenter = (fMin+fMax)*REAL(0.5);
417	// calculate triangles half interval
418	dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
419
420	// if they do not overlap,
421	if( dFabs(fCenter) > ( frc + fTriangleRadius ) )
422	{
423	// exit, we have no intersection
424	return FALSE;
425	}
426
427	// calculate depth
428	dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius);
429
430	// if greater then best found so far
431	if ( fDepth > fBestDepth )
432	{
433	// remember depth
434	fBestDepth = fDepth;
435	fBestCenter = fCenter;
436	fBestrt = fTriangleRadius;
437
438	vNormal[0] = vAxis[0];
439	vNormal[1] = vAxis[1];
440	vNormal[2] = vAxis[2];
441
442	iBestAxis = iAxis;
443
444	// flip normal if interval is wrong faced
445	if (fCenter<0 && !bNoFlip)
446	{
447	vNormal[0] = -vNormal[0];
448	vNormal[1] = -vNormal[1];
449	vNormal[2] = -vNormal[2];
450
451	fBestCenter = -fCenter;
452	}
453	}
454
455	return TRUE;
456	}
457
458	// helper for less key strokes
459	inline void _CalculateAxis(const dVector3& v1,
460	const dVector3& v2,
461	const dVector3& v3,
462	const dVector3& v4,
463	dVector3& r)
464	{
465	dVector3 t1;
466	dVector3 t2;
467
468	SUBTRACT(v1,v2,t1);
469	dCROSS(t2,=,t1,v3);
470	dCROSS(r,=,t2,v4);
471	}
472
473	static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0,
474	const dVector3 &v1,
475	const dVector3 &v2,
476	uint8 flags)
477	{
478	// calculate caps centers in absolute space
479	dVector3 vCp0;
480	vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
481	vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
482	vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
483
484	dVector3 vCp1;
485	vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
486	vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
487	vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
488
489	// reset best axis
490	iBestAxis = 0;
491	// reset best depth
492	fBestDepth = -MAX_REAL;
493	// reset separating axis vector
494	dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
495
496	// Epsilon value for checking axis vector length
497	const dReal fEpsilon = 1e-6f;
498
499	// Translate triangle to Cc cord.
500	SUBTRACT(v0 , vCapsulePosition, vV0);
501	SUBTRACT(v1 , vCapsulePosition, vV1);
502	SUBTRACT(v2 , vCapsulePosition, vV2);
503
504	// We begin to test for 19 separating axis now
505	// I wonder does it help if we employ the method like ISA-GJK???
506	// Or at least we should do experiment and find what axis will
507	// be most likely to be separating axis to check it first.
508
509	// Original
510	// axis vN
511	//vAxis = -vN;
512	vAxis[0] = - vN[0];
513	vAxis[1] = - vN[1];
514	vAxis[2] = - vN[2];
515	if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE))
516	{
517	return FALSE;
518	}
519
520	if (flags & dxTriMeshData::kEdge0)
521	{
522	// axis CxE0 - Edge 0
523	dCROSS(vAxis,=,vCapsuleAxis,vE0);
524	//vAxis = dCROSS( vCapsuleAxis cross vE0 );
525	if( _length2OfVector3( vAxis ) > fEpsilon ) {
526	if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) {
527	return FALSE;
528	}
529	}
530	}
531
532	if (flags & dxTriMeshData::kEdge1)
533	{
534	// axis CxE1 - Edge 1
535	dCROSS(vAxis,=,vCapsuleAxis,vE1);
536	//vAxis = ( vCapsuleAxis cross vE1 );
537	if(_length2OfVector3( vAxis ) > fEpsilon ) {
538	if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) {
539	return FALSE;
540	}
541	}
542	}
543
544	if (flags & dxTriMeshData::kEdge2)
545	{
546	// axis CxE2 - Edge 2
547	//vAxis = ( vCapsuleAxis cross vE2 );
548	dCROSS(vAxis,=,vCapsuleAxis,vE2);
549	if(_length2OfVector3( vAxis ) > fEpsilon ) {
550	if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) {
551	return FALSE;
552	}
553	}
554	}
555
556	if (flags & dxTriMeshData::kEdge0)
557	{
558	// first capsule point
559	// axis ((Cp0-V0) x E0) x E0
560	_CalculateAxis(vCp0,v0,vE0,vE0,vAxis);
561	// vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
562	if(_length2OfVector3( vAxis ) > fEpsilon ) {
563	if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) {
564	return FALSE;
565	}
566	}
567	}
568
569	if (flags & dxTriMeshData::kEdge1)
570	{
571	// axis ((Cp0-V1) x E1) x E1
572	_CalculateAxis(vCp0,v1,vE1,vE1,vAxis);
573	//vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
574	if(_length2OfVector3( vAxis ) > fEpsilon ) {
575	if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) {
576	return FALSE;
577	}
578	}
579	}
580
581	if (flags & dxTriMeshData::kEdge2)
582	{
583	// axis ((Cp0-V2) x E2) x E2
584	_CalculateAxis(vCp0,v2,vE2,vE2,vAxis);
585	//vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
586	if(_length2OfVector3( vAxis ) > fEpsilon ) {
587	if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) {
588	return FALSE;
589	}
590	}
591	}
592
593	if (flags & dxTriMeshData::kEdge0)
594	{
595	// second capsule point
596	// axis ((Cp1-V0) x E0) x E0
597	_CalculateAxis(vCp1,v0,vE0,vE0,vAxis);
598	//vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
599	if(_length2OfVector3( vAxis ) > fEpsilon ) {
600	if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) {
601	return FALSE;
602	}
603	}
604	}
605
606	if (flags & dxTriMeshData::kEdge1)
607	{
608	// axis ((Cp1-V1) x E1) x E1
609	_CalculateAxis(vCp1,v1,vE1,vE1,vAxis);
610	//vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
611	if(_length2OfVector3( vAxis ) > fEpsilon ) {
612	if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) {
613	return FALSE;
614	}
615	}
616	}
617
618	if (flags & dxTriMeshData::kEdge2)
619	{
620	// axis ((Cp1-V2) x E2) x E2
621	_CalculateAxis(vCp1,v2,vE2,vE2,vAxis);
622	//vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
623	if(_length2OfVector3( vAxis ) > fEpsilon ) {
624	if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) {
625	return FALSE;
626	}
627	}
628	}
629
630	if (flags & dxTriMeshData::kVert0)
631	{
632	// first vertex on triangle
633	// axis ((V0-Cp0) x C) x C
634	_CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
635	//vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
636	if(_length2OfVector3( vAxis ) > fEpsilon ) {
637	if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) {
638	return FALSE;
639	}
640	}
641	}
642
643	if (flags & dxTriMeshData::kVert1)
644	{
645	// second vertex on triangle
646	// axis ((V1-Cp0) x C) x C
647	_CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
648	//vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
649	if(_length2OfVector3( vAxis ) > fEpsilon ) {
650	if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) {
651	return FALSE;
652	}
653	}
654	}
655
656	if (flags & dxTriMeshData::kVert2)
657	{
658	// third vertex on triangle
659	// axis ((V2-Cp0) x C) x C
660	_CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
661	//vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
662	if(_length2OfVector3( vAxis ) > fEpsilon ) {
663	if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) {
664	return FALSE;
665	}
666	}
667	}
668
669	// Test as separating axes direction vectors between each triangle
670	// edge and each capsule's cap center
671
672	if (flags & dxTriMeshData::kVert0)
673	{
674	// first triangle vertex and first capsule point
675	//vAxis = v0 - vCp0;
676	SUBTRACT(v0,vCp0,vAxis);
677	if(_length2OfVector3( vAxis ) > fEpsilon ) {
678	if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) {
679	return FALSE;
680	}
681	}
682	}
683
684	if (flags & dxTriMeshData::kVert1)
685	{
686	// second triangle vertex and first capsule point
687	//vAxis = v1 - vCp0;
688	SUBTRACT(v1,vCp0,vAxis);
689	if(_length2OfVector3( vAxis ) > fEpsilon ) {
690	if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) {
691	return FALSE;
692	}
693	}
694	}
695
696	if (flags & dxTriMeshData::kVert2)
697	{
698	// third triangle vertex and first capsule point
699	//vAxis = v2 - vCp0;
700	SUBTRACT(v2,vCp0,vAxis);
701	if(_length2OfVector3( vAxis ) > fEpsilon ) {
702	if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) {
703	return FALSE;
704	}
705	}
706	}
707
708	if (flags & dxTriMeshData::kVert0)
709	{
710	// first triangle vertex and second capsule point
711	//vAxis = v0 - vCp1;
712	SUBTRACT(v0,vCp1,vAxis);
713	if(_length2OfVector3( vAxis ) > fEpsilon ) {
714	if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) {
715	return FALSE;
716	}
717	}
718	}
719
720	if (flags & dxTriMeshData::kVert1)
721	{
722	// second triangle vertex and second capsule point
723	//vAxis = v1 - vCp1;
724	SUBTRACT(v1,vCp1,vAxis);
725	if(_length2OfVector3( vAxis ) > fEpsilon ) {
726	if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) {
727	return FALSE;
728	}
729	}
730	}
731
732	if (flags & dxTriMeshData::kVert2)
733	{
734	// third triangle vertex and second capsule point
735	//vAxis = v2 - vCp1;
736	SUBTRACT(v2,vCp1,vAxis);
737	if(_length2OfVector3( vAxis ) > fEpsilon ) {
738	if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) {
739	return FALSE;
740	}
741	}
742	}
743
744	return TRUE;
745	}
746
747	// test one mesh triangle on intersection with capsule
748	static void _cldTestOneTriangleVSCapsule( const dVector3 &v0,
749	const dVector3 &v1,
750	const dVector3 &v2,
751	uint8 flags)
752	{
753
754	// calculate edges
755	SUBTRACT(v1,v0,vE0);
756	SUBTRACT(v2,v1,vE1);
757	SUBTRACT(v0,v2,vE2);
758
759	dVector3 _minus_vE0;
760	SUBTRACT(v0,v1,_minus_vE0);
761
762	// calculate poly normal
763	dCROSS(vN,=,vE1,_minus_vE0);
764	dNormalize3(vN);
765
766	// create plane from triangle
767	dReal plDistance = -dDOT(v0,vN);
768	dVector4 plTrianglePlane;
769	CONSTRUCTPLANE(plTrianglePlane,vN,plDistance);
770
771	// calculate capsule distance to plane
772	dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition);
773
774	// Capsule must be over positive side of triangle
775	if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
776	{
777	// if not don't generate contacts
778	return;
779	}
780
781	dVector3 vPnt0;
782	SET (vPnt0,v0);
783	dVector3 vPnt1;
784	SET (vPnt1,v1);
785	dVector3 vPnt2;
786	SET (vPnt2,v2);
787
788	if (fDistanceCapsuleCenterToPlane < 0 )
789	{
790	SET (vPnt0,v0);
791	SET (vPnt1,v2);
792	SET (vPnt2,v1);
793	}
794
795	// do intersection test and find best separating axis
796	if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags) )
797	{
798	// if not found do nothing
799	return;
800	}
801
802	// if best separation axis is not found
803	if ( iBestAxis == 0 )
804	{
805	// this should not happen (we should already exit in that case)
806	dIASSERT(FALSE);
807	// do nothing
808	return;
809	}
810
811	// calculate caps centers in absolute space
812	dVector3 vCposTrans;
813	vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius;
814	vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius;
815	vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius;
816
817	dVector3 vCEdgePoint0;
818	vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
819	vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
820	vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
821
822	dVector3 vCEdgePoint1;
823	vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
824	vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
825	vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2](fCapsuleSizeREAL(0.5)-vCapsuleRadius);
826
827	// transform capsule edge points into triangle space
828	vCEdgePoint0[0] -= vPnt0[0];
829	vCEdgePoint0[1] -= vPnt0[1];
830	vCEdgePoint0[2] -= vPnt0[2];
831
832	vCEdgePoint1[0] -= vPnt0[0];
833	vCEdgePoint1[1] -= vPnt0[1];
834	vCEdgePoint1[2] -= vPnt0[2];
835
836	dVector4 plPlane;
837	dVector3 _minus_vN;
838	_minus_vN[0] = -vN[0];
839	_minus_vN[1] = -vN[1];
840	_minus_vN[2] = -vN[2];
841	// triangle plane
842	CONSTRUCTPLANE(plPlane,_minus_vN,0);
843	//plPlane = Plane4f( -vN, 0);
844
845	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
846	{
847	return;
848	}
849
850	// plane with edge 0
851	dVector3 vTemp;
852	dCROSS(vTemp,=,vN,vE0);
853	CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
854	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
855	{
856	return;
857	}
858
859	dCROSS(vTemp,=,vN,vE1);
860	CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-REAL(1e-5)));
861	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
862	{
863	return;
864	}
865
866	dCROSS(vTemp,=,vN,vE2);
867	CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
868	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
869	return;
870	}
871
872	// return capsule edge points into absolute space
873	vCEdgePoint0[0] += vPnt0[0];
874	vCEdgePoint0[1] += vPnt0[1];
875	vCEdgePoint0[2] += vPnt0[2];
876
877	vCEdgePoint1[0] += vPnt0[0];
878	vCEdgePoint1[1] += vPnt0[1];
879	vCEdgePoint1[2] += vPnt0[2];
880
881	// calculate depths for both contact points
882	SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp);
883	dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
884	SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp);
885	dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
886
887	// clamp depths to zero
888	if(fDepth0 < 0)
889	{
890	fDepth0 = 0.0f;
891	}
892
893	if(fDepth1 < 0 )
894	{
895	fDepth1 = 0.0f;
896	}
897
898	// Cached contacts's data
899	// contact 0
900	dIASSERT(ctContacts < (iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
901	gLocalContacts[ctContacts].fDepth = fDepth0;
902	SET(gLocalContacts[ctContacts].vNormal,vNormal);
903	SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0);
904	gLocalContacts[ctContacts].nFlags = 1;
905	ctContacts++;
906
907	if (ctContacts < (iFlags & NUMC_MASK)) {
908	// contact 1
909	gLocalContacts[ctContacts].fDepth = fDepth1;
910	SET(gLocalContacts[ctContacts].vNormal,vNormal);
911	SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1);
912	gLocalContacts[ctContacts].nFlags = 1;
913	ctContacts++;
914	}
915
916	}
917
918	// capsule - trimesh by CroTeam
919	// Ported by Nguyem Binh
920	int dCollideCCTL(dxGeom o1, dxGeom o2, int flags, dContactGeom *contact, int skip)
921	{
922	dIASSERT (skip >= (int)sizeof(dContactGeom));
923	dIASSERT (o1->type == dTriMeshClass);
924	dIASSERT (o2->type == dCapsuleClass);
925	dIASSERT ((flags & NUMC_MASK) >= 1);
926
927	dxTriMesh* TriMesh = (dxTriMesh*)o1;
928	gCylinder = o2;
929	gTriMesh = o1;
930
931	const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder);
932	memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
933
934	const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder);
935	memcpy(vCapsulePosition,pDst,sizeof(dVector3));
936
937	vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
938	vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
939	vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
940
941	// Get size of Capsule
942	dGeomCapsuleGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
943	fCapsuleSize += 2*vCapsuleRadius;
944
945	const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
946	memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3));
947
948	const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
949	memcpy(mTriMeshPos,pTriPos,sizeof(dVector3));
950
951	// global info for contact creation
952	iStride =skip;
953	iFlags =flags;
954	ContactGeoms =contact;
955
956	// reset contact counter
957	ctContacts = 0;
958
959	// reset best depth
960	fBestDepth = - MAX_REAL;
961	fBestCenter = 0;
962	fBestrt = 0;
963
964
965
966
967	// reset collision normal
968	vNormal[0] = REAL(0.0);
969	vNormal[1] = REAL(0.0);
970	vNormal[2] = REAL(0.0);
971
972	// Will it better to use LSS here? -> confirm Pierre.
973	OBBCollider& Collider = TriMesh->_OBBCollider;
974
975	// It is a potential issue to explicitly cast to float
976	// if custom width floating point type is introduced in OPCODE.
977	// It is necessary to make a typedef and cast to it
978	// (e.g. typedef float opc_float;)
979	// However I'm not sure in what header it should be added.
980
981	Point cCenter(/(float)/ vCapsulePosition[0], /(float)/ vCapsulePosition[1], /(float)/ vCapsulePosition[2]);
982	Point cExtents(/(float)/ vCapsuleRadius, /(float)/ vCapsuleRadius,/(float)/ fCapsuleSize/2);
983
984	Matrix3x3 obbRot;
985
986	obbRot[0][0] = /(float)/ mCapsuleRotation[0];
987	obbRot[1][0] = /(float)/ mCapsuleRotation[1];
988	obbRot[2][0] = /(float)/ mCapsuleRotation[2];
989
990	obbRot[0][1] = /(float)/ mCapsuleRotation[4];
991	obbRot[1][1] = /(float)/ mCapsuleRotation[5];
992	obbRot[2][1] = /(float)/ mCapsuleRotation[6];
993
994	obbRot[0][2] = /(float)/ mCapsuleRotation[8];
995	obbRot[1][2] = /(float)/ mCapsuleRotation[9];
996	obbRot[2][2] = /(float)/ mCapsuleRotation[10];
997
998	OBB obbCapsule(cCenter,cExtents,obbRot);
999
1000	Matrix4x4 CapsuleMatrix;
1001	MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
1002
1003	Matrix4x4 MeshMatrix;
1004	MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix);
1005
1006	// TC results
1007	if (TriMesh->doBoxTC) {
1008	dxTriMesh::BoxTC* BoxTC = 0;
1009	for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
1010	if (TriMesh->BoxTCCache[i].Geom == gCylinder){
1011	BoxTC = &TriMesh->BoxTCCache[i];
1012	break;
1013	}
1014	}
1015	if (!BoxTC){
1016	TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
1017
1018	BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
1019	BoxTC->Geom = gCylinder;
1020	BoxTC->FatCoeff = 1.0f;
1021	}
1022
1023	// Intersect
1024	Collider.SetTemporalCoherence(true);
1025	Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
1026	}
1027	else {
1028	Collider.SetTemporalCoherence(false);
1029	Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
1030	}
1031
1032	if (! Collider.GetContactStatus()) {
1033	// no collision occurred
1034	return 0;
1035	}
1036
1037	// Retrieve data
1038	int TriCount = Collider.GetNbTouchedPrimitives();
1039	const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1040
1041	if (TriCount != 0)
1042	{
1043	if (TriMesh->ArrayCallback != null)
1044	{
1045	TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount);
1046	}
1047
1048	// allocate buffer for local contacts on stack
1049	gLocalContacts = (sLocalContactData)dALLOCA16(sizeof(sLocalContactData)(iFlags & NUMC_MASK));
1050
1051	unsigned int ctContacts0 = ctContacts;
1052
1053	uint8* UseFlags = TriMesh->Data->UseFlags;
1054
1055	// loop through all intersecting triangles
1056	for (int i = 0; i < TriCount; i++)
1057	{
1058	const int Triint = Triangles[i];
1059	if (!Callback(TriMesh, gCylinder, Triint)) continue;
1060
1061
1062	dVector3 dv[3];
1063	FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv);
1064
1065	uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll;
1066
1067	// test this triangle
1068	_cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags);
1069
1070	// fill-in tri index for generated contacts
1071	for (; ctContacts0<ctContacts; ctContacts0++)
1072	gLocalContacts[ctContacts0].triIndex = Triint;
1073
1074	// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1075	if(ctContacts>=(iFlags & NUMC_MASK))
1076	{
1077	break;
1078	}
1079
1080	}
1081	}
1082
1083	return _ProcessLocalContacts();
1084	}
1085	#endif
1086
1087	// GIMPACT version
1088	#if dTRIMESH_GIMPACT
1089	#define nCAPSULE_AXIS 2
1090	// capsule - trimesh By francisco leon
1091	int dCollideCCTL(dxGeom o1, dxGeom o2, int flags, dContactGeom *contact, int skip)
1092	{
1093	dIASSERT (skip >= (int)sizeof(dContactGeom));
1094	dIASSERT (o1->type == dTriMeshClass);
1095	dIASSERT (o2->type == dCapsuleClass);
1096	dIASSERT ((flags & NUMC_MASK) >= 1);
1097
1098	dxTriMesh* TriMesh = (dxTriMesh*)o1;
1099	dxGeom* gCylinder = o2;
1100
1101	//Get capsule params
1102	dMatrix3 mCapsuleRotation;
1103	dVector3 vCapsulePosition;
1104	dVector3 vCapsuleAxis;
1105	dReal vCapsuleRadius;
1106	dReal fCapsuleSize;
1107	dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
1108	memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
1109	dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
1110	memcpy(vCapsulePosition,pDst,sizeof(dVector3));
1111	//Axis
1112	vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
1113	vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
1114	vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
1115	// Get size of CCylinder
1116	dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
1117	fCapsuleSize*=0.5f;
1118	//Set Capsule params
1119	GIM_CAPSULE_DATA capsule;
1120
1121	capsule.m_radius = vCapsuleRadius;
1122	VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
1123	VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
1124	VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
1125	VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
1126
1127
1128	//Create contact list
1129	GDYNAMIC_ARRAY trimeshcontacts;
1130	GIM_CREATE_CONTACT_LIST(trimeshcontacts);
1131
1132	//Collide trimeshe vs capsule
1133	gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
1134
1135
1136	if(trimeshcontacts.m_size == 0)
1137	{
1138	GIM_DYNARRAY_DESTROY(trimeshcontacts);
1139	return 0;
1140	}
1141
1142	GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
1143
1144	unsigned contactcount = trimeshcontacts.m_size;
1145	unsigned contactmax = (unsigned)(flags & NUMC_MASK);
1146	if (contactcount > contactmax)
1147	{
1148	contactcount = contactmax;
1149	}
1150
1151	dContactGeom* pcontact;
1152	unsigned i;
1153
1154	for (i=0;i<contactcount;i++)
1155	{
1156	pcontact = SAFECONTACT(flags, contact, i, skip);
1157
1158	pcontact->pos[0] = ptrimeshcontacts->m_point[0];
1159	pcontact->pos[1] = ptrimeshcontacts->m_point[1];
1160	pcontact->pos[2] = ptrimeshcontacts->m_point[2];
1161	pcontact->pos[3] = 1.0f;
1162
1163	pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
1164	pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
1165	pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
1166	pcontact->normal[3] = 0;
1167
1168	pcontact->depth = ptrimeshcontacts->m_depth;
1169	pcontact->g1 = TriMesh;
1170	pcontact->g2 = gCylinder;
1171
1172	ptrimeshcontacts++;
1173	}
1174
1175	GIM_DYNARRAY_DESTROY(trimeshcontacts);
1176
1177	return (int)contactcount;
1178	}
1179	#endif
1180
1181	#endif // dTRIMESH_ENABLED

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

Download in other formats: