Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp @ 3327

Last change on this file since 3327 was 2662, checked in by rgrieder, 16 years ago
Merged presentation branch back to trunk.
Property svn:eol-style set to `native`
File size: 15.6 KB

Line
1
2	/*! \file gim_tri_collision.h
3	\author Francisco Len Nßjera
4	*/
5	/*
6	-----------------------------------------------------------------------------
7	This source file is part of GIMPACT Library.
8
9	For the latest info, see http://gimpact.sourceforge.net/
10
11	Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
12	email: projectileman@yahoo.com
13
14	This library is free software; you can redistribute it and/or
15	modify it under the terms of EITHER:
16	(1) The GNU Lesser General Public License as published by the Free
17	Software Foundation; either version 2.1 of the License, or (at
18	your option) any later version. The text of the GNU Lesser
19	General Public License is included with this library in the
20	file GIMPACT-LICENSE-LGPL.TXT.
21	(2) The BSD-style license that is included with this library in
22	the file GIMPACT-LICENSE-BSD.TXT.
23	(3) The zlib/libpng license that is included with this library in
24	the file GIMPACT-LICENSE-ZLIB.TXT.
25
26	This library is distributed in the hope that it will be useful,
27	but WITHOUT ANY WARRANTY; without even the implied warranty of
28	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
29	GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
30
31	-----------------------------------------------------------------------------
32	*/
33
34	#include "gim_tri_collision.h"
35
36
37	#define TRI_LOCAL_EPSILON 0.000001f
38	#define MIN_EDGE_EDGE_DIS 0.00001f
39
40
41	class GIM_TRIANGLE_CALCULATION_CACHE
42	{
43	public:
44	GREAL margin;
45	btVector3 tu_vertices[3];
46	btVector3 tv_vertices[3];
47	btVector4 tu_plane;
48	btVector4 tv_plane;
49	btVector3 closest_point_u;
50	btVector3 closest_point_v;
51	btVector3 edge_edge_dir;
52	btVector3 distances;
53	GREAL du[4];
54	GREAL du0du1;
55	GREAL du0du2;
56	GREAL dv[4];
57	GREAL dv0dv1;
58	GREAL dv0dv2;
59	btVector3 temp_points[MAX_TRI_CLIPPING];
60	btVector3 temp_points1[MAX_TRI_CLIPPING];
61	btVector3 contact_points[MAX_TRI_CLIPPING];
62
63
64
65	//! if returns false, the faces are paralele
66	SIMD_FORCE_INLINE bool compute_intervals(
67	const GREAL &D0,
68	const GREAL &D1,
69	const GREAL &D2,
70	const GREAL &D0D1,
71	const GREAL &D0D2,
72	GREAL & scale_edge0,
73	GREAL & scale_edge1,
74	GUINT &edge_index0,
75	GUINT &edge_index1)
76	{
77	if(D0D1>0.0f)
78	{
79	/* here we know that D0D2<=0.0 */
80	/* that is D0, D1 are on the same side, D2 on the other or on the plane */
81	scale_edge0 = -D2/(D0-D2);
82	scale_edge1 = -D1/(D2-D1);
83	edge_index0 = 2;edge_index1 = 1;
84	}
85	else if(D0D2>0.0f)
86	{
87	/* here we know that d0d1<=0.0 */
88	scale_edge0 = -D0/(D1-D0);
89	scale_edge1 = -D1/(D2-D1);
90	edge_index0 = 0;edge_index1 = 1;
91	}
92	else if(D1*D2>0.0f \|\| D0!=0.0f)
93	{
94	/* here we know that d0d1<=0.0 or that D0!=0.0 */
95	scale_edge0 = -D0/(D1-D0);
96	scale_edge1 = -D2/(D0-D2);
97	edge_index0 = 0 ;edge_index1 = 2;
98	}
99	else
100	{
101	return false;
102	}
103	return true;
104	}
105
106
107	//! clip triangle
108	/*!
109	*/
110	SIMD_FORCE_INLINE GUINT clip_triangle(
111	const btVector4 & tri_plane,
112	const btVector3 * tripoints,
113	const btVector3 * srcpoints,
114	btVector3 * clip_points)
115	{
116	// edge 0
117
118	btVector4 edgeplane;
119
120	EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
121
122	GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
123	edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
124
125	if(clipped_count == 0) return 0;
126
127	// edge 1
128
129	EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
130
131	clipped_count = PLANE_CLIP_POLYGON3D(
132	edgeplane,temp_points,clipped_count,temp_points1);
133
134	if(clipped_count == 0) return 0;
135
136	// edge 2
137
138	EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
139
140	clipped_count = PLANE_CLIP_POLYGON3D(
141	edgeplane,temp_points1,clipped_count,clip_points);
142
143	return clipped_count;
144
145
146	/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
147	GUINT i1 = (i0+1)%3;
148	// edge 0
149	btVector3 temp_points[MAX_TRI_CLIPPING];
150	btVector3 temp_points1[MAX_TRI_CLIPPING];
151
152	GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
153	0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
154	DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
155
156
157	if(clipped_count == 0) return 0;
158
159	// edge 1
160	clipped_count = PLANE_CLIP_POLYGON_GENERIC(
161	0,temp_points,clipped_count,temp_points1,
162	DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
163
164	if(clipped_count == 0) return 0;
165
166	// edge 2
167	clipped_count = PLANE_CLIP_POLYGON_GENERIC(
168	0,temp_points1,clipped_count,clipped_points,
169	DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
170
171	return clipped_count;*/
172	}
173
174	SIMD_FORCE_INLINE void sort_isect(
175	GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
176	{
177	if(isect1<isect0)
178	{
179	//swap
180	GIM_SWAP_NUMBERS(isect0,isect1);
181	GIM_SWAP_NUMBERS(e0,e1);
182	btVector3 tmp = vec0;
183	vec0 = vec1;
184	vec1 = tmp;
185	}
186	}
187
188	//! Test verifying interval intersection with the direction between planes
189	/*!
190	\pre tv_plane and tu_plane must be set
191	\post
192	distances[2] is set with the distance
193	closest_point_u, closest_point_v, edge_edge_dir are set too
194	\return
195	- 0: faces are paralele
196	- 1: face U casts face V
197	- 2: face V casts face U
198	- 3: nearest edges
199	*/
200	SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
201	{
202	// Compute direction of intersection line
203	edge_edge_dir = tu_plane.cross(tv_plane);
204	GREAL Dlen;
205	VEC_LENGTH(edge_edge_dir,Dlen);
206
207	if(Dlen<0.0001)
208	{
209	return 0; //faces near paralele
210	}
211
212	edge_edge_dir*= 1/Dlen;//normalize
213
214
215	// Compute interval for triangle 1
216	GUINT tu_e0,tu_e1;//edge indices
217	GREAL tu_scale_e0,tu_scale_e1;//edge scale
218	if(!compute_intervals(du[0],du[1],du[2],
219	du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
220
221	// Compute interval for triangle 2
222	GUINT tv_e0,tv_e1;//edge indices
223	GREAL tv_scale_e0,tv_scale_e1;//edge scale
224
225	if(!compute_intervals(dv[0],dv[1],dv[2],
226	dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
227
228	//proyected vertices
229	btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
230	btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
231
232	btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
233	btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
234
235	//proyected intervals
236	GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
237	GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
238
239	sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
240	sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
241
242	const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
243	const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
244
245	if(midpoint_u<midpoint_v)
246	{
247	if(isect_u[1]>=isect_v[1]) // face U casts face V
248	{
249	return 1;
250	}
251	else if(isect_v[0]<=isect_u[0]) // face V casts face U
252	{
253	return 2;
254	}
255	// closest points
256	closest_point_u = up_e1;
257	closest_point_v = vp_e0;
258	// calc edges and separation
259
260	if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
261	{
262	SEGMENT_COLLISION(
263	tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
264	tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
265	closest_point_u,
266	closest_point_v);
267
268	edge_edge_dir = closest_point_u-closest_point_v;
269	VEC_LENGTH(edge_edge_dir,distances[2]);
270	edge_edge_dir *= 1.0f/distances[2];// normalize
271	}
272	else
273	{
274	distances[2] = isect_v[0]-isect_u[1];//distance negative
275	//edge_edge_dir *= -1.0f; //normal pointing from V to U
276	}
277
278	}
279	else
280	{
281	if(isect_v[1]>=isect_u[1]) // face V casts face U
282	{
283	return 2;
284	}
285	else if(isect_u[0]<=isect_v[0]) // face U casts face V
286	{
287	return 1;
288	}
289	// closest points
290	closest_point_u = up_e0;
291	closest_point_v = vp_e1;
292	// calc edges and separation
293
294	if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
295	{
296	SEGMENT_COLLISION(
297	tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
298	tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
299	closest_point_u,
300	closest_point_v);
301
302	edge_edge_dir = closest_point_u-closest_point_v;
303	VEC_LENGTH(edge_edge_dir,distances[2]);
304	edge_edge_dir *= 1.0f/distances[2];// normalize
305	}
306	else
307	{
308	distances[2] = isect_u[0]-isect_v[1];//distance negative
309	//edge_edge_dir *= -1.0f; //normal pointing from V to U
310	}
311	}
312	return 3;
313	}
314
315
316	//! collides by two sides
317	SIMD_FORCE_INLINE bool triangle_collision(
318	const btVector3 & u0,
319	const btVector3 & u1,
320	const btVector3 & u2,
321	GREAL margin_u,
322	const btVector3 & v0,
323	const btVector3 & v1,
324	const btVector3 & v2,
325	GREAL margin_v,
326	GIM_TRIANGLE_CONTACT_DATA & contacts)
327	{
328
329	margin = margin_u + margin_v;
330
331	tu_vertices[0] = u0;
332	tu_vertices[1] = u1;
333	tu_vertices[2] = u2;
334
335	tv_vertices[0] = v0;
336	tv_vertices[1] = v1;
337	tv_vertices[2] = v2;
338
339	//create planes
340	// plane v vs U points
341
342	TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
343
344	du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
345	du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
346	du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
347
348
349	du0du1 = du[0] * du[1];
350	du0du2 = du[0] * du[2];
351
352
353	if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
354	{
355	if(du[0]<0) //we need test behind the triangle plane
356	{
357	distances[0] = GIM_MAX3(du[0],du[1],du[2]);
358	distances[0] = -distances[0];
359	if(distances[0]>margin) return false; //never intersect
360
361	//reorder triangle v
362	VEC_SWAP(tv_vertices[0],tv_vertices[1]);
363	VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
364	}
365	else
366	{
367	distances[0] = GIM_MIN3(du[0],du[1],du[2]);
368	if(distances[0]>margin) return false; //never intersect
369	}
370	}
371	else
372	{
373	//Look if we need to invert the triangle
374	distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
375
376	if(distances[0]<0.0f)
377	{
378	//reorder triangle v
379	VEC_SWAP(tv_vertices[0],tv_vertices[1]);
380	VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
381
382	distances[0] = GIM_MAX3(du[0],du[1],du[2]);
383	distances[0] = -distances[0];
384	}
385	else
386	{
387	distances[0] = GIM_MIN3(du[0],du[1],du[2]);
388	}
389	}
390
391
392	// plane U vs V points
393
394	TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
395
396	dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
397	dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
398	dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
399
400	dv0dv1 = dv[0] * dv[1];
401	dv0dv2 = dv[0] * dv[2];
402
403
404	if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
405	{
406	if(dv[0]<0) //we need test behind the triangle plane
407	{
408	distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
409	distances[1] = -distances[1];
410	if(distances[1]>margin) return false; //never intersect
411
412	//reorder triangle u
413	VEC_SWAP(tu_vertices[0],tu_vertices[1]);
414	VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
415	}
416	else
417	{
418	distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
419	if(distances[1]>margin) return false; //never intersect
420	}
421	}
422	else
423	{
424	//Look if we need to invert the triangle
425	distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
426
427	if(distances[1]<0.0f)
428	{
429	//reorder triangle v
430	VEC_SWAP(tu_vertices[0],tu_vertices[1]);
431	VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
432
433	distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
434	distances[1] = -distances[1];
435	}
436	else
437	{
438	distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
439	}
440	}
441
442	GUINT bl;
443	/* bl = cross_line_intersection_test();
444	if(bl==3)
445	{
446	//take edge direction too
447	bl = distances.maxAxis();
448	}
449	else
450	{*/
451	bl = 0;
452	if(distances[0]<distances[1]) bl = 1;
453	//}
454
455	if(bl==2) //edge edge separation
456	{
457	if(distances[2]>margin) return false;
458
459	contacts.m_penetration_depth = -distances[2] + margin;
460	contacts.m_points[0] = closest_point_v;
461	contacts.m_point_count = 1;
462	VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
463
464	return true;
465	}
466
467	//clip face against other
468
469
470	GUINT point_count;
471	//TODO
472	if(bl == 0) //clip U points against V
473	{
474	point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
475	if(point_count == 0) return false;
476	contacts.merge_points(tv_plane,margin,contact_points,point_count);
477	}
478	else //clip V points against U
479	{
480	point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
481	if(point_count == 0) return false;
482	contacts.merge_points(tu_plane,margin,contact_points,point_count);
483	contacts.m_separating_normal *= -1.f;
484	}
485	if(contacts.m_point_count == 0) return false;
486	return true;
487	}
488
489	};
490
491
492	/*class GIM_TRIANGLE_CALCULATION_CACHE
493	{
494	public:
495	GREAL margin;
496	GUINT clipped_count;
497	btVector3 tu_vertices[3];
498	btVector3 tv_vertices[3];
499	btVector3 temp_points[MAX_TRI_CLIPPING];
500	btVector3 temp_points1[MAX_TRI_CLIPPING];
501	btVector3 clipped_points[MAX_TRI_CLIPPING];
502	GIM_TRIANGLE_CONTACT_DATA contacts1;
503	GIM_TRIANGLE_CONTACT_DATA contacts2;
504
505
506	//! clip triangle
507	GUINT clip_triangle(
508	const btVector4 & tri_plane,
509	const btVector3 * tripoints,
510	const btVector3 * srcpoints,
511	btVector3 * clipped_points)
512	{
513	// edge 0
514
515	btVector4 edgeplane;
516
517	EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
518
519	GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
520	edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
521
522	if(clipped_count == 0) return 0;
523
524	// edge 1
525
526	EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
527
528	clipped_count = PLANE_CLIP_POLYGON3D(
529	edgeplane,temp_points,clipped_count,temp_points1);
530
531	if(clipped_count == 0) return 0;
532
533	// edge 2
534
535	EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
536
537	clipped_count = PLANE_CLIP_POLYGON3D(
538	edgeplane,temp_points1,clipped_count,clipped_points);
539
540	return clipped_count;
541	}
542
543
544
545
546	//! collides only on one side
547	bool triangle_collision(
548	const btVector3 & u0,
549	const btVector3 & u1,
550	const btVector3 & u2,
551	GREAL margin_u,
552	const btVector3 & v0,
553	const btVector3 & v1,
554	const btVector3 & v2,
555	GREAL margin_v,
556	GIM_TRIANGLE_CONTACT_DATA & contacts)
557	{
558
559	margin = margin_u + margin_v;
560
561
562	tu_vertices[0] = u0;
563	tu_vertices[1] = u1;
564	tu_vertices[2] = u2;
565
566	tv_vertices[0] = v0;
567	tv_vertices[1] = v1;
568	tv_vertices[2] = v2;
569
570	//create planes
571	// plane v vs U points
572
573
574	TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
575
576	clipped_count = clip_triangle(
577	contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
578
579	if(clipped_count == 0 )
580	{
581	return false;//Reject
582	}
583
584	//find most deep interval face1
585	contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
586	if(contacts1.m_point_count == 0) return false; // too far
587
588	//Normal pointing to triangle1
589	//contacts1.m_separating_normal *= -1.f;
590
591	//Clip tri1 by tri2 edges
592
593	TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
594
595	clipped_count = clip_triangle(
596	contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
597
598	if(clipped_count == 0 )
599	{
600	return false;//Reject
601	}
602
603	//find most deep interval face1
604	contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
605	if(contacts2.m_point_count == 0) return false; // too far
606
607	contacts2.m_separating_normal *= -1.f;
608
609	////check most dir for contacts
610	if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
611	{
612	contacts.copy_from(contacts2);
613	}
614	else
615	{
616	contacts.copy_from(contacts1);
617	}
618	return true;
619	}
620
621
622	};*/
623
624
625
626	bool GIM_TRIANGLE::collide_triangle_hard_test(
627	const GIM_TRIANGLE & other,
628	GIM_TRIANGLE_CONTACT_DATA & contact_data) const
629	{
630	GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
631	return calc_cache.triangle_collision(
632	m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
633	other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
634	contact_data);
635
636	}
637
638
639
640

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

Download in other formats: