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

Rev	Line
[2431]	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: