Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/src/collision_trimesh_trimesh_new.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: 32.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	// OPCODE TriMesh/TriMesh collision code
24	// Written at 2006-10-28 by Francisco León (http://gimpact.sourceforge.net)
25
26	#ifdef _MSC_VER
27	#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28	#endif
29
30	#include <ode/collision.h>
31	#include <ode/matrix.h>
32	#include <ode/rotation.h>
33	#include <ode/odemath.h>
34
35	// New Implementation
36	#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER
37
38	#if dTRIMESH_ENABLED
39
40	#include "collision_util.h"
41	#define TRIMESH_INTERNAL
42	#include "collision_trimesh_internal.h"
43
44	#if dTRIMESH_OPCODE
45
46	#define SMALL_ELT REAL(2.5e-4)
47	#define EXPANDED_ELT_THRESH REAL(1.0e-3)
48	#define DISTANCE_EPSILON REAL(1.0e-8)
49	#define VELOCITY_EPSILON REAL(1.0e-5)
50	#define TINY_PENETRATION REAL(5.0e-6)
51
52	struct LineContactSet
53	{
54	enum
55	{
56	MAX_POINTS = 8
57	};
58
59	dVector3 Points[MAX_POINTS];
60	int Count;
61	};
62
63
64	static void GetTriangleGeometryCallback(udword, VertexPointers&, udword);
65	inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
66	static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
67	static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
68	static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
69	static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
70	const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
71	dReal t,dReal u,dReal *v);
72
73
74	///returns the penetration depth
75	static dReal MostDeepPoints(
76	LineContactSet & points,
77	const dVector3 plane_normal,
78	dReal plane_dist,
79	LineContactSet & deep_points);
80	///returns the penetration depth
81	static dReal FindMostDeepPointsInTetra(
82	LineContactSet contact_points,
83	const dVector3 sourcetri[3],///triangle which contains contact_points
84	const dVector3 tetra[4],
85	const dVector4 tetraplanes[4],
86	dVector3 separating_normal,
87	LineContactSet deep_points);
88
89	static bool ClipTriByTetra(const dVector3 tri[3],
90	const dVector3 tetra[4],
91	LineContactSet& Contacts);
92	static bool TriTriContacts(const dVector3 tr1[3],
93	const dVector3 tr2[3],
94	dxGeom* g1, dxGeom* g2, int Flags,
95	dContactGeom* Contacts, int Stride,
96	int &contactcount);
97
98
99	/* some math macros */
100	#define CROSS(dest,v1,v2) { dest[0]=v1[1]v2[2]-v1[2]v2[1]; \
101	dest[1]=v1[2]v2[0]-v1[0]v2[2]; \
102	dest[2]=v1[0]v2[1]-v1[1]v2[0]; }
103
104	#define DOT(v1,v2) (v1[0]v2[0]+v1[1]v2[1]+v1[2]*v2[2])
105
106	#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }
107
108	#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }
109
110	#define MULT(dest,v,factor) { dest[0]=factorv[0]; dest[1]=factorv[1]; dest[2]=factor*v[2]; }
111
112	#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }
113
114	#define SMULT(p,q,s) { p[0]=q[0]s; p[1]=q[1]s; p[2]=q[2]*s; }
115
116	#define COMBO(combo,p,t,q) { combo[0]=p[0]+tq[0]; combo[1]=p[1]+tq[1]; combo[2]=p[2]+t*q[2]; }
117
118	#define LENGTH(x) ((dReal) 1.0f/InvSqrt(dDOT(x, x)))
119
120	#define DEPTH(d, p, q, n) d = (p[0] - q[0])n[0] + (p[1] - q[1])n[1] + (p[2] - q[2])*n[2];
121
122	inline const dReal dMin(const dReal x, const dReal y)
123	{
124	return x < y ? x : y;
125	}
126
127
128	inline void
129	SwapNormals(dVector3 &pen_v, dVector3 &col_v, dVector3* v1, dVector3* v2,
130	dVector3 &pen_elt, dVector3 elt_f1, dVector3 *elt_f2,
131	dVector3 n, dVector3 n1, dVector3 n2)
132	{
133	if (pen_v == v1) {
134	pen_v = v2;
135	pen_elt = elt_f2;
136	col_v = v1;
137	SET(n, n1);
138	}
139	else {
140	pen_v = v1;
141	pen_elt = elt_f1;
142	col_v = v2;
143	SET(n, n2);
144	}
145	}
146
147	///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
148	////* Written by Francisco León (http://gimpact.sourceforge.net) *///
149	#define CONTACT_DIFF_EPSILON REAL(0.00001)
150	#if defined(dDOUBLE)
151	#define CONTACT_NORMAL_ZERO REAL(0.0000001)
152	#else // if defined(dSINGLE)
153	#define CONTACT_NORMAL_ZERO REAL(0.00001)
154	#endif
155	#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
156	#define dSQRT3 REAL(1.7320508075688773)
157
158	struct CONTACT_KEY
159	{
160	dContactGeom * m_contact;
161	unsigned int m_key;
162	};
163
164	#define MAXCONTACT_X_NODE 4
165	struct CONTACT_KEY_HASH_NODE
166	{
167	CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
168	int m_keycount;
169	};
170
171	#define CONTACTS_HASHSIZE 256
172	CONTACT_KEY_HASH_NODE g_hashcontactset[CONTACTS_HASHSIZE];
173
174
175
176	void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
177	{
178	key.m_contact = contact;
179
180	unsigned int hash=0;
181
182	int i = 0;
183
184	while (true)
185	{
186	dReal coord = contact->pos[i];
187	coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);
188
189	unsigned int hash_input = ((unsigned int *)&coord)[0];
190	if (sizeof(dReal) / sizeof(unsigned int) != 1)
191	{
192	dIASSERT(sizeof(dReal) / sizeof(unsigned int) == 2);
193	hash_input ^= ((unsigned int *)&coord)[1];
194	}
195
196	hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
197	hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
198	hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
199	hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );
200
201	if (++i == 3)
202	{
203	break;
204	}
205
206	hash = (hash << 11) \| (hash >> 21);
207	}
208
209	key.m_key = hash;
210	}
211
212
213	static inline unsigned int MakeContactIndex(unsigned int key)
214	{
215	dIASSERT(CONTACTS_HASHSIZE == 256);
216
217	unsigned int index = key ^ (key >> 16);
218	index = (index ^ (index >> 8)) & 0xFF;
219	return index;
220	}
221
222	dContactGeom AddContactToNode(const CONTACT_KEY contactkey,CONTACT_KEY_HASH_NODE * node)
223	{
224	for(int i=0;i<node->m_keycount;i++)
225	{
226	if(node->m_keyarray[i].m_key == contactkey->m_key)
227	{
228	dContactGeom *contactfound = node->m_keyarray[i].m_contact;
229	if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /for comp. errors/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /cube diagonal/)
230	{
231	return contactfound;
232	}
233	}
234	}
235
236	if (node->m_keycount < MAXCONTACT_X_NODE)
237	{
238	node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;
239	node->m_keyarray[node->m_keycount].m_key = contactkey->m_key;
240	node->m_keycount++;
241	}
242	else
243	{
244	dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled");
245	}
246
247	return contactkey->m_contact;
248	}
249
250	void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node)
251	{
252	dIASSERT(node->m_keycount > 0);
253
254	if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact)
255	{
256	node->m_keycount -= 1;
257	}
258	else
259	{
260	dIASSERT(node->m_keycount == MAXCONTACT_X_NODE);
261	}
262	}
263
264	void RemoveArbitraryContactFromNode(const CONTACT_KEY contactkey, CONTACT_KEY_HASH_NODE node)
265	{
266	dIASSERT(node->m_keycount > 0);
267
268	int keyindex, lastkeyindex = node->m_keycount - 1;
269
270	// Do not check the last contact
271	for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
272	{
273	if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
274	{
275	node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex];
276	break;
277	}
278	}
279
280	dIASSERT(keyindex < lastkeyindex \|\|
281	node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
282
283	node->m_keycount = lastkeyindex;
284	}
285
286	void UpdateArbitraryContactInNode(const CONTACT_KEY contactkey, CONTACT_KEY_HASH_NODE node,
287	dContactGeom *pwithcontact)
288	{
289	dIASSERT(node->m_keycount > 0);
290
291	int keyindex, lastkeyindex = node->m_keycount - 1;
292
293	// Do not check the last contact
294	for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
295	{
296	if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
297	{
298	break;
299	}
300	}
301
302	dIASSERT(keyindex < lastkeyindex \|\|
303	node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
304
305	node->m_keyarray[keyindex].m_contact = pwithcontact;
306	}
307
308	void ClearContactSet()
309	{
310	memset(g_hashcontactset,0,sizeof(CONTACT_KEY_HASH_NODE)*CONTACTS_HASHSIZE);
311	}
312
313	//return true if found
314	dContactGeom *InsertContactInSet(const CONTACT_KEY &newkey)
315	{
316	unsigned int index = MakeContactIndex(newkey.m_key);
317
318	return AddContactToNode(&newkey, &g_hashcontactset[index]);
319	}
320
321	void RemoveNewContactFromSet(const CONTACT_KEY &contactkey)
322	{
323	unsigned int index = MakeContactIndex(contactkey.m_key);
324
325	RemoveNewContactFromNode(&contactkey, &g_hashcontactset[index]);
326	}
327
328	void RemoveArbitraryContactFromSet(const CONTACT_KEY &contactkey)
329	{
330	unsigned int index = MakeContactIndex(contactkey.m_key);
331
332	RemoveArbitraryContactFromNode(&contactkey, &g_hashcontactset[index]);
333	}
334
335	void UpdateArbitraryContactInSet(const CONTACT_KEY &contactkey,
336	dContactGeom *pwithcontact)
337	{
338	unsigned int index = MakeContactIndex(contactkey.m_key);
339
340	UpdateArbitraryContactInNode(&contactkey, &g_hashcontactset[index], pwithcontact);
341	}
342
343	bool AllocNewContact(
344	const dVector3 newpoint, dContactGeom *& out_pcontact,
345	int Flags, dContactGeom* Contacts,
346	int Stride, int &contactcount)
347	{
348	bool allocated_new = false;
349
350	dContactGeom dLocalContact;
351
352	dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ?
353	SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact;
354
355	pcontact->pos[0] = newpoint[0];
356	pcontact->pos[1] = newpoint[1];
357	pcontact->pos[2] = newpoint[2];
358	pcontact->pos[3] = 1.0f;
359
360	CONTACT_KEY newkey;
361	UpdateContactKey(newkey, pcontact);
362
363	dContactGeom *pcontactfound = InsertContactInSet(newkey);
364	if (pcontactfound == pcontact)
365	{
366	if (pcontactfound != &dLocalContact)
367	{
368	contactcount++;
369	}
370	else
371	{
372	RemoveNewContactFromSet(newkey);
373	pcontactfound = NULL;
374	}
375
376	allocated_new = true;
377	}
378
379	out_pcontact = pcontactfound;
380	return allocated_new;
381	}
382
383	void FreeExistingContact(dContactGeom *pcontact,
384	int Flags, dContactGeom *Contacts,
385	int Stride, int &contactcount)
386	{
387	CONTACT_KEY contactKey;
388	UpdateContactKey(contactKey, pcontact);
389
390	RemoveArbitraryContactFromSet(contactKey);
391
392	int lastContactIndex = contactcount - 1;
393	dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride);
394
395	if (pcontact != plastContact)
396	{
397	pcontact = plastContact;
398
399	CONTACT_KEY lastContactKey;
400	UpdateContactKey(lastContactKey, plastContact);
401
402	UpdateArbitraryContactInSet(lastContactKey, pcontact);
403	}
404
405	contactcount = lastContactIndex;
406	}
407
408
409	dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2,
410	const dVector3 point,
411	dVector3 normal,
412	dReal depth,
413	int Flags,
414	dContactGeom* Contacts, int Stride,
415	int &contactcount)
416	{
417	dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code
418
419	dContactGeom * pcontact;
420
421	if (!AllocNewContact(point, pcontact, Flags, Contacts, Stride, contactcount))
422	{
423	const dReal depthDifference = depth - pcontact->depth;
424
425	if (depthDifference > CONTACT_DIFF_EPSILON)
426	{
427	pcontact->normal[0] = normal[0];
428	pcontact->normal[1] = normal[1];
429	pcontact->normal[2] = normal[2];
430	pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
431	pcontact->depth = depth;
432
433	pcontact->g1 = g1;
434	pcontact->g2 = g2;
435	}
436	else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average
437	{
438	if(pcontact->g1 == g2)
439	{
440	MULT(normal,normal, REAL(-1.0));
441	}
442
443	const dReal oldLen = pcontact->normal[3];
444	COMBO(pcontact->normal, normal, oldLen, pcontact->normal);
445
446	const dReal len = LENGTH(pcontact->normal);
447	if (len > CONTACT_NORMAL_ZERO)
448	{
449	MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len);
450	pcontact->normal[3] = len;
451	}
452	else
453	{
454	FreeExistingContact(pcontact, Flags, Contacts, Stride, contactcount);
455	}
456	}
457	}
458	// Contact can be not available if buffer is full
459	else if (pcontact)
460	{
461	pcontact->normal[0] = normal[0];
462	pcontact->normal[1] = normal[1];
463	pcontact->normal[2] = normal[2];
464	pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
465	pcontact->depth = depth;
466	pcontact->g1 = g1;
467	pcontact->g2 = g2;
468	}
469
470	return pcontact;
471	}
472
473	////////////////////////////////////////////////////////////////////////////////////////////
474
475
476
477
478
479	int
480	dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
481	{
482	dIASSERT (Stride >= (int)sizeof(dContactGeom));
483	dIASSERT (g1->type == dTriMeshClass);
484	dIASSERT (g2->type == dTriMeshClass);
485	dIASSERT ((Flags & NUMC_MASK) >= 1);
486
487	dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
488	dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
489
490	dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
491	dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
492
493	const dVector3& TLPosition1 = (const dVector3) dGeomGetPosition(TriMesh1);
494	// TLRotation1 = column-major order
495	const dMatrix3& TLRotation1 = (const dMatrix3) dGeomGetRotation(TriMesh1);
496
497	const dVector3& TLPosition2 = (const dVector3) dGeomGetPosition(TriMesh2);
498	// TLRotation2 = column-major order
499	const dMatrix3& TLRotation2 = (const dMatrix3) dGeomGetRotation(TriMesh2);
500
501	AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider;
502
503
504	static BVTCache ColCache;
505	ColCache.Model0 = &TriMesh1->Data->BVTree;
506	ColCache.Model1 = &TriMesh2->Data->BVTree;
507
508	////Prepare contact list
509	ClearContactSet();
510
511	// Collision query
512	Matrix4x4 amatrix, bmatrix;
513	BOOL IsOk = Collider.Collide(ColCache,
514	&MakeMatrix(TLPosition1, TLRotation1, amatrix),
515	&MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
516
517
518	// Make "double" versions of these matrices, if appropriate
519	dMatrix4 A, B;
520	dMakeMatrix4(TLPosition1, TLRotation1, A);
521	dMakeMatrix4(TLPosition2, TLRotation2, B);
522
523
524
525
526	if (IsOk) {
527	// Get collision status => if true, objects overlap
528	if ( Collider.GetContactStatus() ) {
529	// Number of colliding pairs and list of pairs
530	int TriCount = Collider.GetNbPairs();
531	const Pair* CollidingPairs = Collider.GetPairs();
532
533	if (TriCount > 0) {
534	// step through the pairs, adding contacts
535	int id1, id2;
536	int OutTriCount = 0;
537	dVector3 v1[3], v2[3];
538
539	// only do these expensive inversions once
540	/*dMatrix4 InvMatrix1, InvMatrix2;
541	dInvertMatrix4(A, InvMatrix1);
542	dInvertMatrix4(B, InvMatrix2);*/
543
544
545	for (int i = 0; i < TriCount; i++)
546	{
547	id1 = CollidingPairs[i].id0;
548	id2 = CollidingPairs[i].id1;
549
550	// grab the colliding triangles
551	FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
552	FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
553	// Since we'll be doing matrix transformations, we need to
554	// make sure that all vertices have four elements
555	for (int j=0; j<3; j++) {
556	v1[j][3] = 1.0;
557	v2[j][3] = 1.0;
558	}
559
560	TriTriContacts(v1,v2,
561	g1, g2, Flags,
562	Contacts,Stride,OutTriCount);
563
564	// Continue loop even after contacts are full
565	// as existing contacts' normals/depths might be updated
566	// Break only if contacts are not important
567	if ((OutTriCount \| CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK \| CONTACTS_UNIMPORTANT)))
568	{
569	break;
570	}
571	}
572
573	// Return the number of contacts
574	return OutTriCount;
575
576	}
577	}
578	}
579
580
581	// There was some kind of failure during the Collide call or
582	// there are no faces overlapping
583	return 0;
584	}
585
586
587
588	static void
589	GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
590	{
591	dVector3 Out[3];
592
593	FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
594
595	for (int i = 0; i < 3; i++)
596	triangle.Vertex[i] = (const Point) ((dReal) Out[i]);
597	}
598
599
600	//
601	//
602	//
603	#define B11 B[0]
604	#define B12 B[1]
605	#define B13 B[2]
606	#define B14 B[3]
607	#define B21 B[4]
608	#define B22 B[5]
609	#define B23 B[6]
610	#define B24 B[7]
611	#define B31 B[8]
612	#define B32 B[9]
613	#define B33 B[10]
614	#define B34 B[11]
615	#define B41 B[12]
616	#define B42 B[13]
617	#define B43 B[14]
618	#define B44 B[15]
619
620	#define Binv11 Binv[0]
621	#define Binv12 Binv[1]
622	#define Binv13 Binv[2]
623	#define Binv14 Binv[3]
624	#define Binv21 Binv[4]
625	#define Binv22 Binv[5]
626	#define Binv23 Binv[6]
627	#define Binv24 Binv[7]
628	#define Binv31 Binv[8]
629	#define Binv32 Binv[9]
630	#define Binv33 Binv[10]
631	#define Binv34 Binv[11]
632	#define Binv41 Binv[12]
633	#define Binv42 Binv[13]
634	#define Binv43 Binv[14]
635	#define Binv44 Binv[15]
636
637	inline void
638	dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
639	{
640	B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
641	B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
642	B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
643
644	B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
645	}
646
647
648	static void
649	dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
650	{
651	dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
652	-(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
653	+(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
654	+(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
655	-(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
656	+(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
657
658	dAASSERT (det != 0.0);
659
660	det = 1.0 / det;
661
662	Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
663	Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
664	Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
665	Binv14 = 0.0f;
666	Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
667	Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
668	Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
669	Binv24 = 0.0f;
670	Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
671	Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
672	Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
673	Binv34 = 0.0f;
674	Binv41 = (dReal) (det * (B21(B33B42 - B32B43) + B22(B31B43 - B33B41) + B23(B32B41 - B31*B42)));
675	Binv42 = (dReal) (det * (B31(B13B42 - B12B43) + B32(B11B43 - B13B41) + B33(B12B41 - B11*B42)));
676	Binv43 = (dReal) (det * (B41(B13B22 - B12B23) + B42(B11B23 - B13B21) + B43(B12B21 - B11*B22)));
677	Binv44 = 1.0f;
678	}
679
680
681
682	// Find the intersectiojn point between a coplanar line segement,
683	// defined by X1 and X2, and a ray defined by X3 and direction N.
684	//
685	// This forumla for this calculation is:
686	// (c x b) . (a x b)
687	// Q = x1 + a -------------------
688	// \| a x b \| ^2
689	//
690	// where a = x2 - x1
691	// b = x4 - x3
692	// c = x3 - x1
693	// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
694	// and x4 is (CoplanarPt - n)
695	static int
696	IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n,
697	dVector3 out_pt)
698	{
699	dVector3 a, b, c, x4;
700
701	ADD(x4, x3, n); // x4 = x3 + n
702
703	SUB(a, x2, x1); // a = x2 - x1
704	SUB(b, x4, x3);
705	SUB(c, x3, x1);
706
707	dVector3 tmp1, tmp2;
708	CROSS(tmp1, c, b);
709	CROSS(tmp2, a, b);
710
711	dReal num, denom;
712	num = dDOT(tmp1, tmp2);
713	denom = LENGTH( tmp2 );
714
715	dReal s;
716	s = num /(denom*denom);
717
718	for (int i=0; i<3; i++)
719	out_pt[i] = x1[i] + a[i]*s;
720
721	// Test if this intersection is "behind" x3, w.r.t. n
722	SUB(a, x3, out_pt);
723	if (dDOT(a, n) > 0.0)
724	return 0;
725
726	// Test if this intersection point is outside the edge limits,
727	// if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside
728	// else outside
729	SUB(a, out_pt, x1);
730	SUB(b, out_pt, x2);
731	if (dDOT(a,b) < 0.0)
732	return 1;
733	else
734	return 0;
735	}
736
737
738
739	void PlaneClipSegment( const dVector3 s1, const dVector3 s2,
740	const dVector3 N, dReal C, dVector3 clipped)
741	{
742	dReal dis1,dis2;
743	dis1 = DOT(s1,N)-C;
744	SUB(clipped,s2,s1);
745	dis2 = DOT(clipped,N);
746	MULT(clipped,clipped,-dis1/dis2);
747	ADD(clipped,clipped,s1);
748	clipped[3] = 1.0f;
749	}
750
751	/* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by
752	CONTACTS, with a plane (described by N and C distance from origin).
753	Note: the input vertices are assumed to be in invcounterclockwise order.
754	changed by Francisco Leon (http://gimpact.sourceforge.net) */
755	static void
756	ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C,
757	LineContactSet& Contacts )
758	{
759	int i, vi, prevclassif=32000, classif;
760	/*
761	classif 0 : back, 1 : front
762	*/
763
764	dReal d;
765	dVector3 clipped[8];
766	int clippedcount =0;
767
768	if(Contacts.Count==0)
769	{
770	return;
771	}
772	for(i=0;i<=Contacts.Count;i++)
773	{
774	vi = i%Contacts.Count;
775
776	d = DOT(N,Contacts.Points[vi]) - C;
777	////classify point
778	if(d>REAL(1.0e-8)) classif = 1;
779	else classif = 0;
780
781	if(classif == 0)//back
782	{
783	if(i>0)
784	{
785	if(prevclassif==1)///in front
786	{
787
788	///add clipped point
789	if(clippedcount<8)
790	{
791	PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
792	N,C,clipped[clippedcount]);
793	clippedcount++;
794	}
795	}
796	}
797	///add point
798	if(clippedcount<8&&i<Contacts.Count)
799	{
800	clipped[clippedcount][0] = Contacts.Points[vi][0];
801	clipped[clippedcount][1] = Contacts.Points[vi][1];
802	clipped[clippedcount][2] = Contacts.Points[vi][2];
803	clipped[clippedcount][3] = 1.0f;
804	clippedcount++;
805	}
806	}
807	else
808	{
809
810	if(i>0)
811	{
812	if(prevclassif==0)
813	{
814	///add point
815	if(clippedcount<8)
816	{
817	PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
818	N,C,clipped[clippedcount]);
819	clippedcount++;
820	}
821	}
822	}
823	}
824
825	prevclassif = classif;
826	}
827
828	if(clippedcount==0)
829	{
830	Contacts.Count = 0;
831	return;
832	}
833	Contacts.Count = clippedcount;
834	memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) );
835	return;
836	}
837
838
839	bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2,
840	dVector3 Normal, dReal & Dist)
841	{
842	dVector3 e0,e1;
843	SUB(e0,s1,s0);
844	SUB(e1,s2,s0);
845
846	CROSS(Normal,e0,e1);
847
848	if (!dSafeNormalize3(Normal))
849	{
850	return false;
851	}
852
853	Dist = DOT(Normal,s0);
854	return true;
855
856	}
857
858	bool BuildEdgesDir(const dVector3 s0, const dVector3 s1,
859	const dVector3 t0, const dVector3 t1,
860	dVector3 crossdir)
861	{
862	dVector3 e0,e1;
863
864	SUB(e0,s1,s0);
865	SUB(e1,t1,t0);
866	CROSS(crossdir,e0,e1);
867
868	if (!dSafeNormalize3(crossdir))
869	{
870	return false;
871	}
872	return true;
873	}
874
875
876
877	bool BuildEdgePlane(
878	const dVector3 s0, const dVector3 s1,
879	const dVector3 normal,
880	dVector3 plane_normal,
881	dReal & plane_dist)
882	{
883	dVector3 e0;
884
885	SUB(e0,s1,s0);
886	CROSS(plane_normal,e0,normal);
887	if (!dSafeNormalize3(plane_normal))
888	{
889	return false;
890	}
891	plane_dist = DOT(plane_normal,s0);
892	return true;
893	}
894
895
896
897
898	/*
899	Positive penetration
900	Negative number: they are separated
901	*/
902	dReal IntervalPenetration(dReal &vmin1,dReal &vmax1,
903	dReal &vmin2,dReal &vmax2)
904	{
905	if(vmax1<=vmin2)
906	{
907	return -(vmin2-vmax1);
908	}
909	else
910	{
911	if(vmax2<=vmin1)
912	{
913	return -(vmin1-vmax2);
914	}
915	else
916	{
917	if(vmax1<=vmax2)
918	{
919	return vmax1-vmin2;
920	}
921
922	return vmax2-vmin1;
923	}
924
925	}
926	return 0;
927	}
928
929	void FindInterval(
930	const dVector3 * vertices, int verticecount,
931	dVector3 dir,dReal &vmin,dReal &vmax)
932	{
933
934	dReal dist;
935	int i;
936	vmin = DOT(vertices[0],dir);
937	vmax = vmin;
938	for(i=1;i<verticecount;i++)
939	{
940	dist = DOT(vertices[i],dir);
941	if(vmin>dist) vmin=dist;
942	else if(vmax<dist) vmax=dist;
943	}
944	}
945
946	///returns the penetration depth
947	dReal MostDeepPoints(
948	LineContactSet & points,
949	const dVector3 plane_normal,
950	dReal plane_dist,
951	LineContactSet & deep_points)
952	{
953	int i;
954	int max_candidates[8];
955	dReal maxdeep=-dInfinity;
956	dReal dist;
957
958	deep_points.Count = 0;
959	for(i=0;i<points.Count;i++)
960	{
961	dist = DOT(plane_normal,points.Points[i]) - plane_dist;
962	dist *= -1.0f;
963	if(dist>maxdeep)
964	{
965	maxdeep = dist;
966	deep_points.Count=1;
967	max_candidates[deep_points.Count-1] = i;
968	}
969	else if(dist+REAL(0.000001)>=maxdeep)
970	{
971	deep_points.Count++;
972	max_candidates[deep_points.Count-1] = i;
973	}
974	}
975
976	for(i=0;i<deep_points.Count;i++)
977	{
978	SET(deep_points.Points[i],points.Points[max_candidates[i]]);
979	}
980	return maxdeep;
981
982	}
983
984	void ClipPointsByTri(
985	const dVector3 * points, int pointcount,
986	const dVector3 tri[3],
987	const dVector3 triplanenormal,
988	dReal triplanedist,
989	LineContactSet & clipped_points,
990	bool triplane_clips)
991	{
992	///build edges planes
993	int i;
994	dVector4 plane;
995
996	clipped_points.Count = pointcount;
997	memcpy(&clipped_points.Points[0],&points[0],pointcount*sizeof(dVector3));
998	for(i=0;i<3;i++)
999	{
1000	if (BuildEdgePlane(
1001	tri[i],tri[(i+1)%3],triplanenormal,
1002	plane,plane[3]))
1003	{
1004	ClipConvexPolygonAgainstPlane(
1005	plane,
1006	plane[3],
1007	clipped_points);
1008	}
1009	}
1010
1011	if(triplane_clips)
1012	{
1013	ClipConvexPolygonAgainstPlane(
1014	triplanenormal,
1015	triplanedist,
1016	clipped_points);
1017	}
1018	}
1019
1020
1021	///returns the penetration depth
1022	dReal FindTriangleTriangleCollision(
1023	const dVector3 tri1[3],
1024	const dVector3 tri2[3],
1025	dVector3 separating_normal,
1026	LineContactSet & deep_points)
1027	{
1028	dReal maxdeep=dInfinity;
1029	dReal dist;
1030	int mostdir=0,mostface = 0, currdir=0;
1031	// dReal vmin1,vmax1,vmin2,vmax2;
1032	// dVector3 crossdir, pt1,pt2;
1033	dVector4 tri1plane,tri2plane;
1034	separating_normal[3] = 0.0f;
1035	bool bl;
1036	LineContactSet clipped_points1,clipped_points2;
1037	LineContactSet deep_points1,deep_points2;
1038	////find interval face1
1039
1040	bl = BuildPlane(tri1[0],tri1[1],tri1[2],
1041	tri1plane,tri1plane[3]);
1042	clipped_points1.Count = 0;
1043
1044	if(bl)
1045	{
1046	ClipPointsByTri(
1047	tri2, 3,
1048	tri1,
1049	tri1plane,
1050	tri1plane[3],
1051	clipped_points1,false);
1052
1053
1054
1055	maxdeep = MostDeepPoints(
1056	clipped_points1,
1057	tri1plane,
1058	tri1plane[3],
1059	deep_points1);
1060	SET(separating_normal,tri1plane);
1061
1062	}
1063	currdir++;
1064
1065	////find interval face2
1066
1067	bl = BuildPlane(tri2[0],tri2[1],tri2[2],
1068	tri2plane,tri2plane[3]);
1069
1070
1071	clipped_points2.Count = 0;
1072	if(bl)
1073	{
1074	ClipPointsByTri(
1075	tri1, 3,
1076	tri2,
1077	tri2plane,
1078	tri2plane[3],
1079	clipped_points2,false);
1080
1081
1082
1083	dist = MostDeepPoints(
1084	clipped_points2,
1085	tri2plane,
1086	tri2plane[3],
1087	deep_points2);
1088
1089
1090
1091	if(dist<maxdeep)
1092	{
1093	maxdeep = dist;
1094	mostdir = currdir;
1095	mostface = 1;
1096	SET(separating_normal,tri2plane);
1097	}
1098	}
1099	currdir++;
1100
1101
1102	///find edge edge distances
1103	///test each edge plane
1104
1105	/*for(i=0;i<3;i++)
1106	{
1107
1108
1109	for(j=0;j<3;j++)
1110	{
1111
1112
1113	bl = BuildEdgesDir(
1114	tri1[i],tri1[(i+1)%3],
1115	tri2[j],tri2[(j+1)%3],
1116	crossdir);
1117
1118	////find plane distance
1119
1120	if(bl)
1121	{
1122	FindInterval(tri1,3,crossdir,vmin1,vmax1);
1123	FindInterval(tri2,3,crossdir,vmin2,vmax2);
1124
1125	dist = IntervalPenetration(
1126	vmin1,
1127	vmax1,
1128	vmin2,
1129	vmax2);
1130	if(dist<maxdeep)
1131	{
1132	maxdeep = dist;
1133	mostdir = currdir;
1134	SET(separating_normal,crossdir);
1135	}
1136	}
1137	currdir++;
1138	}
1139	}*/
1140
1141
1142	////check most dir for contacts
1143	if(mostdir==0)
1144	{
1145	///find most deep points
1146	deep_points.Count = deep_points1.Count;
1147	memcpy(
1148	&deep_points.Points[0],
1149	&deep_points1.Points[0],
1150	deep_points1.Count*sizeof(dVector3));
1151
1152	///invert normal for point to tri1
1153	MULT(separating_normal,separating_normal,-1.0f);
1154	}
1155	else if(mostdir==1)
1156	{
1157	deep_points.Count = deep_points2.Count;
1158	memcpy(
1159	&deep_points.Points[0],
1160	&deep_points2.Points[0],
1161	deep_points2.Count*sizeof(dVector3));
1162
1163	}
1164	/*else
1165	{///edge separation
1166	mostdir -= 2;
1167
1168	//edge 2
1169	j = mostdir%3;
1170	//edge 1
1171	i = mostdir/3;
1172
1173	///find edge closest points
1174	dClosestLineSegmentPoints(
1175	tri1[i],tri1[(i+1)%3],
1176	tri2[j],tri2[(j+1)%3],
1177	pt1,pt2);
1178	///find correct direction
1179
1180	SUB(crossdir,pt2,pt1);
1181
1182	vmin1 = LENGTH(crossdir);
1183	if(vmin1<REAL(0.000001))
1184	{
1185
1186	if(mostface==0)
1187	{
1188	vmin1 = DOT(separating_normal,tri1plane);
1189	if(vmin1>0.0)
1190	{
1191	MULT(separating_normal,separating_normal,-1.0f);
1192	deep_points.Count = 1;
1193	SET(deep_points.Points[0],pt2);
1194	}
1195	else
1196	{
1197	deep_points.Count = 1;
1198	SET(deep_points.Points[0],pt2);
1199	}
1200
1201	}
1202	else
1203	{
1204	vmin1 = DOT(separating_normal,tri2plane);
1205	if(vmin1<0.0)
1206	{
1207	MULT(separating_normal,separating_normal,-1.0f);
1208	deep_points.Count = 1;
1209	SET(deep_points.Points[0],pt2);
1210	}
1211	else
1212	{
1213	deep_points.Count = 1;
1214	SET(deep_points.Points[0],pt2);
1215	}
1216
1217	}
1218
1219
1220
1221
1222	}
1223	else
1224	{
1225	MULT(separating_normal,crossdir,1.0f/vmin1);
1226
1227	vmin1 = DOT(separating_normal,tri1plane);
1228	if(vmin1>0.0)
1229	{
1230	MULT(separating_normal,separating_normal,-1.0f);
1231	deep_points.Count = 1;
1232	SET(deep_points.Points[0],pt2);
1233	}
1234	else
1235	{
1236	deep_points.Count = 1;
1237	SET(deep_points.Points[0],pt2);
1238	}
1239
1240
1241	}
1242
1243
1244	}*/
1245	return maxdeep;
1246	}
1247
1248
1249
1250	///SUPPORT UP TO 8 CONTACTS
1251	bool TriTriContacts(const dVector3 tr1[3],
1252	const dVector3 tr2[3],
1253	dxGeom* g1, dxGeom* g2, int Flags,
1254	dContactGeom* Contacts, int Stride,
1255	int &contactcount)
1256	{
1257
1258
1259	dVector4 normal;
1260	dReal depth;
1261	///Test Tri Vs Tri
1262	// dContactGeom* pcontact;
1263	int ccount = 0;
1264	LineContactSet contactpoints;
1265	contactpoints.Count = 0;
1266
1267
1268
1269	///find best direction
1270
1271	depth = FindTriangleTriangleCollision(
1272	tr1,
1273	tr2,
1274	normal,
1275	contactpoints);
1276
1277
1278
1279	if(depth<0.0f) return false;
1280
1281	ccount = 0;
1282	while (ccount<contactpoints.Count)
1283	{
1284	PushNewContact( g1, g2,
1285	contactpoints.Points[ccount],
1286	normal, depth, Flags,
1287	Contacts,Stride,contactcount);
1288
1289	// Continue loop even after contacts are full
1290	// as existing contacts' normals/depths might be updated
1291	// Break only if contacts are not important
1292	if ((contactcount \| CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK \| CONTACTS_UNIMPORTANT)))
1293	{
1294	break;
1295	}
1296
1297	ccount++;
1298	}
1299	return true;
1300	}
1301
1302	#endif // dTRIMESH_OPCODE
1303	#endif // dTRIMESH_USE_NEW_TRIMESH_TRIMESH_COLLIDER
1304	#endif // dTRIMESH_ENABLED

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

Download in other formats: