MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp - llvm-test-suite - Git at Google


 /*! \file gim_tri_collision.h
 \author Francisco Len Nßjera
 */
 /*
 -----------------------------------------------------------------------------
 This source file is part of GIMPACT Library.

 For the latest info, see http://gimpact.sourceforge.net/

 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
 email: projectileman@yahoo.com

  This library is free software; you can redistribute it and/or
  modify it under the terms of EITHER:
    (1) The GNU Lesser General Public License as published by the Free
        Software Foundation; either version 2.1 of the License, or (at
        your option) any later version. The text of the GNU Lesser
        General Public License is included with this library in the
        file GIMPACT-LICENSE-LGPL.TXT.
    (2) The BSD-style license that is included with this library in
        the file GIMPACT-LICENSE-BSD.TXT.
    (3) The zlib/libpng license that is included with this library in
        the file GIMPACT-LICENSE-ZLIB.TXT.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

 -----------------------------------------------------------------------------
 */

 #include "BulletCollision/Gimpact/gim_tri_collision.h"


 #define TRI_LOCAL_EPSILON 0.000001f
 #define MIN_EDGE_EDGE_DIS 0.00001f


 class GIM_TRIANGLE_CALCULATION_CACHE
 {
 public:
 	GREAL margin;
 	btVector3 tu_vertices[3];
 	btVector3 tv_vertices[3];
 	btVector4 tu_plane;
 	btVector4 tv_plane;
 	btVector3 closest_point_u;
 	btVector3 closest_point_v;
 	btVector3 edge_edge_dir;
 	btVector3 distances;
 	GREAL du[4];
 	GREAL du0du1;
 	GREAL du0du2;
 	GREAL dv[4];
 	GREAL dv0dv1;
 	GREAL dv0dv2;
 	btVector3 temp_points[MAX_TRI_CLIPPING];
 	btVector3 temp_points1[MAX_TRI_CLIPPING];
 	btVector3 contact_points[MAX_TRI_CLIPPING];


 	//! if returns false, the faces are paralele
 	SIMD_FORCE_INLINE bool compute_intervals(
 					const GREAL &D0,
 					const GREAL &D1,
 					const GREAL &D2,
 					const GREAL &D0D1,
 					const GREAL &D0D2,
 					GREAL & scale_edge0,
 					GREAL & scale_edge1,
 					GUINT &edge_index0,
 					GUINT &edge_index1)
 	{
 		if(D0D1>0.0f)
 		{
 			/* here we know that D0D2<=0.0 */
 			/* that is D0, D1 are on the same side, D2 on the other or on the plane */
 			scale_edge0 = -D2/(D0-D2);
 			scale_edge1 = -D1/(D2-D1);
 			edge_index0 = 2;edge_index1 = 1;
 		}
 		else if(D0D2>0.0f)
 		{
 			/* here we know that d0d1<=0.0 */
 			scale_edge0 = -D0/(D1-D0);
 			scale_edge1 = -D1/(D2-D1);
 			edge_index0 = 0;edge_index1 = 1;
 		}
 		else if(D1*D2>0.0f || D0!=0.0f)
 		{
 			/* here we know that d0d1<=0.0 or that D0!=0.0 */
 			scale_edge0 = -D0/(D1-D0);
 			scale_edge1 = -D2/(D0-D2);
 			edge_index0 = 0 ;edge_index1 = 2;
 		}
 		else
 		{
 			return false;
 		}
 		return true;
 	}


 	//! clip triangle
 	/*!
 	*/
 	SIMD_FORCE_INLINE GUINT clip_triangle(
 		const btVector4 & tri_plane,
 		const btVector3 * tripoints,
 		const btVector3 * srcpoints,
 		btVector3 * clip_points)
 	{
 		// edge 0

 		btVector4 edgeplane;

 		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);

 		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
 			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);

 		if(clipped_count == 0) return 0;

 		// edge 1

 		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);

 		clipped_count = PLANE_CLIP_POLYGON3D(
 			edgeplane,temp_points,clipped_count,temp_points1);

 		if(clipped_count == 0) return 0;

 		// edge 2

 		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);

 		clipped_count = PLANE_CLIP_POLYGON3D(
 			edgeplane,temp_points1,clipped_count,clip_points);

 		return clipped_count;


 		/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
 		GUINT i1 = (i0+1)%3;
 		// edge 0
 		btVector3 temp_points[MAX_TRI_CLIPPING];
 		btVector3 temp_points1[MAX_TRI_CLIPPING];

 		GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
 			0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
 			DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));


 		if(clipped_count == 0) return 0;

 		// edge 1
 		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
 			0,temp_points,clipped_count,temp_points1,
 			DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));

 		if(clipped_count == 0) return 0;

 		// edge 2
 		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
 			0,temp_points1,clipped_count,clipped_points,
 			DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));

 		return clipped_count;*/
 	}

 	SIMD_FORCE_INLINE void sort_isect(
 		GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
 	{
 		if(isect1<isect0)
 		{
 			//swap
 			GIM_SWAP_NUMBERS(isect0,isect1);
 			GIM_SWAP_NUMBERS(e0,e1);
 			btVector3 tmp = vec0;
 			vec0 = vec1;
 			vec1 = tmp;
 		}
 	}

 	//! Test verifying interval intersection with the direction between planes
 	/*!
 	\pre tv_plane and tu_plane must be set
 	\post
 	distances[2] is set with the distance
 	closest_point_u, closest_point_v, edge_edge_dir are set too
 	\return
 	- 0: faces are paralele
 	- 1: face U casts face V
 	- 2: face V casts face U
 	- 3: nearest edges
 	*/
 	SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
 	{
 		// Compute direction of intersection line
 		edge_edge_dir = tu_plane.cross(tv_plane);
 		GREAL Dlen;
 		VEC_LENGTH(edge_edge_dir,Dlen);

 		if(Dlen<0.0001)
 		{
 			return 0; //faces near paralele
 		}

 		edge_edge_dir*= 1/Dlen;//normalize


 		// Compute interval for triangle 1
 		GUINT tu_e0,tu_e1;//edge indices
 		GREAL tu_scale_e0,tu_scale_e1;//edge scale
 		if(!compute_intervals(du[0],du[1],du[2],
 			du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;

 		// Compute interval for triangle 2
 		GUINT tv_e0,tv_e1;//edge indices
 		GREAL tv_scale_e0,tv_scale_e1;//edge scale

 		if(!compute_intervals(dv[0],dv[1],dv[2],
 			dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;

 		//proyected vertices
 		btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
 		btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);

 		btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
 		btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);

 		//proyected intervals
 		GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
 		GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};

 		sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
 		sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);

 		const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
 		const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint

 		if(midpoint_u<midpoint_v)
 		{
 			if(isect_u[1]>=isect_v[1]) // face U casts face V
 			{
 				return 1;
 			}
 			else if(isect_v[0]<=isect_u[0]) // face V casts face U
 			{
 				return 2;
 			}
 			// closest points
 			closest_point_u = up_e1;
 			closest_point_v = vp_e0;
 			// calc edges and separation

 			if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
 			{
 				SEGMENT_COLLISION(
 					tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
 					tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
 					closest_point_u,
 					closest_point_v);

 				edge_edge_dir = closest_point_u-closest_point_v;
 				VEC_LENGTH(edge_edge_dir,distances[2]);
 				edge_edge_dir *= 1.0f/distances[2];// normalize
 			}
 			else
 			{
 				distances[2] = isect_v[0]-isect_u[1];//distance negative
 				//edge_edge_dir *= -1.0f; //normal pointing from V to U
 			}

 		}
 		else
 		{
 			if(isect_v[1]>=isect_u[1]) // face V casts face U
 			{
 				return 2;
 			}
 			else if(isect_u[0]<=isect_v[0]) // face U casts face V
 			{
 				return 1;
 			}
 			// closest points
 			closest_point_u = up_e0;
 			closest_point_v = vp_e1;
 			// calc edges and separation

 			if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
 			{
 				SEGMENT_COLLISION(
 					tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
 					tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
 					closest_point_u,
 					closest_point_v);

 				edge_edge_dir = closest_point_u-closest_point_v;
 				VEC_LENGTH(edge_edge_dir,distances[2]);
 				edge_edge_dir *= 1.0f/distances[2];// normalize
 			}
 			else
 			{
 				distances[2] = isect_u[0]-isect_v[1];//distance negative
 				//edge_edge_dir *= -1.0f; //normal pointing from V to U
 			}
 		}
 		return 3;
 	}


 	//! collides by two sides
 	SIMD_FORCE_INLINE bool triangle_collision(
 					const btVector3 & u0,
 					const btVector3 & u1,
 					const btVector3 & u2,
 					GREAL margin_u,
 					const btVector3 & v0,
 					const btVector3 & v1,
 					const btVector3 & v2,
 					GREAL margin_v,
 					GIM_TRIANGLE_CONTACT_DATA & contacts)
 	{

 		margin = margin_u + margin_v;

 		tu_vertices[0] = u0;
 		tu_vertices[1] = u1;
 		tu_vertices[2] = u2;

 		tv_vertices[0] = v0;
 		tv_vertices[1] = v1;
 		tv_vertices[2] = v2;

 		//create planes
 		// plane v vs U points

 		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);

 		du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
 		du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
 		du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);


 		du0du1 = du[0] * du[1];
 		du0du2 = du[0] * du[2];


 		if(du0du1>0.0f && du0du2>0.0f)	// same sign on all of them + not equal 0 ?
 		{
 			if(du[0]<0) //we need test behind the triangle plane
 			{
 				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
 				distances[0] = -distances[0];
 				if(distances[0]>margin) return false; //never intersect

 				//reorder triangle v
 				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
 				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
 			}
 			else
 			{
 				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
 				if(distances[0]>margin) return false; //never intersect
 			}
 		}
 		else
 		{
 			//Look if we need to invert the triangle
 			distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid

 			if(distances[0]<0.0f)
 			{
 				//reorder triangle v
 				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
 				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);

 				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
 				distances[0] = -distances[0];
 			}
 			else
 			{
 				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
 			}
 		}


 		// plane U vs V points

 		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);

 		dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
 		dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
 		dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);

 		dv0dv1 = dv[0] * dv[1];
 		dv0dv2 = dv[0] * dv[2];


 		if(dv0dv1>0.0f && dv0dv2>0.0f)	// same sign on all of them + not equal 0 ?
 		{
 			if(dv[0]<0) //we need test behind the triangle plane
 			{
 				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
 				distances[1] = -distances[1];
 				if(distances[1]>margin) return false; //never intersect

 				//reorder triangle u
 				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
 				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
 			}
 			else
 			{
 				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
 				if(distances[1]>margin) return false; //never intersect
 			}
 		}
 		else
 		{
 			//Look if we need to invert the triangle
 			distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid

 			if(distances[1]<0.0f)
 			{
 				//reorder triangle v
 				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
 				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);

 				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
 				distances[1] = -distances[1];
 			}
 			else
 			{
 				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
 			}
 		}

 		GUINT bl;
 		/* bl = cross_line_intersection_test();
 		if(bl==3)
 		{
 			//take edge direction too
 			bl = distances.maxAxis();
 		}
 		else
 		{*/
 			bl = 0;
 			if(distances[0]<distances[1]) bl = 1;
 		//}

 		if(bl==2) //edge edge separation
 		{
 			if(distances[2]>margin) return false;

 			contacts.m_penetration_depth = -distances[2] + margin;
 			contacts.m_points[0] = closest_point_v;
 			contacts.m_point_count = 1;
 			VEC_COPY(contacts.m_separating_normal,edge_edge_dir);

 			return true;
 		}

 		//clip face against other


 		GUINT point_count;
 		//TODO
 		if(bl == 0) //clip U points against V
 		{
 			point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
 			if(point_count == 0) return false;
 			contacts.merge_points(tv_plane,margin,contact_points,point_count);
 		}
 		else //clip V points against U
 		{
 			point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
 			if(point_count == 0) return false;
 			contacts.merge_points(tu_plane,margin,contact_points,point_count);
 			contacts.m_separating_normal *= -1.f;
 		}
 		if(contacts.m_point_count == 0) return false;
 		return true;
 	}

 };


 /*class GIM_TRIANGLE_CALCULATION_CACHE
 {
 public:
 	GREAL margin;
 	GUINT clipped_count;
 	btVector3 tu_vertices[3];
 	btVector3 tv_vertices[3];
 	btVector3 temp_points[MAX_TRI_CLIPPING];
 	btVector3 temp_points1[MAX_TRI_CLIPPING];
 	btVector3 clipped_points[MAX_TRI_CLIPPING];
 	GIM_TRIANGLE_CONTACT_DATA contacts1;
 	GIM_TRIANGLE_CONTACT_DATA contacts2;


 	//! clip triangle
 	GUINT clip_triangle(
 		const btVector4 & tri_plane,
 		const btVector3 * tripoints,
 		const btVector3 * srcpoints,
 		btVector3 * clipped_points)
 	{
 		// edge 0

 		btVector4 edgeplane;

 		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);

 		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
 			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);

 		if(clipped_count == 0) return 0;

 		// edge 1

 		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);

 		clipped_count = PLANE_CLIP_POLYGON3D(
 			edgeplane,temp_points,clipped_count,temp_points1);

 		if(clipped_count == 0) return 0;

 		// edge 2

 		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);

 		clipped_count = PLANE_CLIP_POLYGON3D(
 			edgeplane,temp_points1,clipped_count,clipped_points);

 		return clipped_count;
 	}


 	//! collides only on one side
 	bool triangle_collision(
 					const btVector3 & u0,
 					const btVector3 & u1,
 					const btVector3 & u2,
 					GREAL margin_u,
 					const btVector3 & v0,
 					const btVector3 & v1,
 					const btVector3 & v2,
 					GREAL margin_v,
 					GIM_TRIANGLE_CONTACT_DATA & contacts)
 	{

 		margin = margin_u + margin_v;


 		tu_vertices[0] = u0;
 		tu_vertices[1] = u1;
 		tu_vertices[2] = u2;

 		tv_vertices[0] = v0;
 		tv_vertices[1] = v1;
 		tv_vertices[2] = v2;

 		//create planes
 		// plane v vs U points


 		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);

 		clipped_count = clip_triangle(
 			contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);

 		if(clipped_count == 0 )
 		{
 			 return false;//Reject
 		}

 		//find most deep interval face1
 		contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
 		if(contacts1.m_point_count == 0) return false; // too far

 		//Normal pointing to triangle1
 		//contacts1.m_separating_normal *= -1.f;

 		//Clip tri1 by tri2 edges

 		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);

 		clipped_count = clip_triangle(
 			contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);

 		if(clipped_count == 0 )
 		{
 			 return false;//Reject
 		}

 		//find most deep interval face1
 		contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
 		if(contacts2.m_point_count == 0) return false; // too far

 		contacts2.m_separating_normal *= -1.f;

 		////check most dir for contacts
 		if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
 		{
 			contacts.copy_from(contacts2);
 		}
 		else
 		{
 			contacts.copy_from(contacts1);
 		}
 		return true;
 	}


 };*/


 bool GIM_TRIANGLE::collide_triangle_hard_test(
 		const GIM_TRIANGLE & other,
 		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
 {
 	GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
 	return calc_cache.triangle_collision(
 					m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
 					other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
 					contact_data);

 }

	/*! \file gim_tri_collision.h
	\author Francisco Len Nßjera
	*/
	/*
	-----------------------------------------------------------------------------
	This source file is part of GIMPACT Library.

	For the latest info, see http://gimpact.sourceforge.net/

	Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
	email: projectileman@yahoo.com

	This library is free software; you can redistribute it and/or
	modify it under the terms of EITHER:
	(1) The GNU Lesser General Public License as published by the Free
	Software Foundation; either version 2.1 of the License, or (at
	your option) any later version. The text of the GNU Lesser
	General Public License is included with this library in the
	file GIMPACT-LICENSE-LGPL.TXT.
	(2) The BSD-style license that is included with this library in
	the file GIMPACT-LICENSE-BSD.TXT.
	(3) The zlib/libpng license that is included with this library in
	the file GIMPACT-LICENSE-ZLIB.TXT.

	This library is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
	GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

	-----------------------------------------------------------------------------
	*/

	#include "BulletCollision/Gimpact/gim_tri_collision.h"


	#define TRI_LOCAL_EPSILON 0.000001f
	#define MIN_EDGE_EDGE_DIS 0.00001f


	class GIM_TRIANGLE_CALCULATION_CACHE
	{
	public:
	GREAL margin;
	btVector3 tu_vertices[3];
	btVector3 tv_vertices[3];
	btVector4 tu_plane;
	btVector4 tv_plane;
	btVector3 closest_point_u;
	btVector3 closest_point_v;
	btVector3 edge_edge_dir;
	btVector3 distances;
	GREAL du[4];
	GREAL du0du1;
	GREAL du0du2;
	GREAL dv[4];
	GREAL dv0dv1;
	GREAL dv0dv2;
	btVector3 temp_points[MAX_TRI_CLIPPING];
	btVector3 temp_points1[MAX_TRI_CLIPPING];
	btVector3 contact_points[MAX_TRI_CLIPPING];



	//! if returns false, the faces are paralele
	SIMD_FORCE_INLINE bool compute_intervals(
	const GREAL &D0,
	const GREAL &D1,
	const GREAL &D2,
	const GREAL &D0D1,
	const GREAL &D0D2,
	GREAL & scale_edge0,
	GREAL & scale_edge1,
	GUINT &edge_index0,
	GUINT &edge_index1)
	{
	if(D0D1>0.0f)
	{
	/* here we know that D0D2<=0.0 */
	/* that is D0, D1 are on the same side, D2 on the other or on the plane */
	scale_edge0 = -D2/(D0-D2);
	scale_edge1 = -D1/(D2-D1);
	edge_index0 = 2;edge_index1 = 1;
	}
	else if(D0D2>0.0f)
	{
	/* here we know that d0d1<=0.0 */
	scale_edge0 = -D0/(D1-D0);
	scale_edge1 = -D1/(D2-D1);
	edge_index0 = 0;edge_index1 = 1;
	}
	else if(D1*D2>0.0f \|\| D0!=0.0f)
	{
	/* here we know that d0d1<=0.0 or that D0!=0.0 */
	scale_edge0 = -D0/(D1-D0);
	scale_edge1 = -D2/(D0-D2);
	edge_index0 = 0 ;edge_index1 = 2;
	}
	else
	{
	return false;
	}
	return true;
	}


	//! clip triangle
	/*!
	*/
	SIMD_FORCE_INLINE GUINT clip_triangle(
	const btVector4 & tri_plane,
	const btVector3 * tripoints,
	const btVector3 * srcpoints,
	btVector3 * clip_points)
	{
	// edge 0

	btVector4 edgeplane;

	EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);

	GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
	edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);

	if(clipped_count == 0) return 0;

	// edge 1

	EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);

	clipped_count = PLANE_CLIP_POLYGON3D(
	edgeplane,temp_points,clipped_count,temp_points1);

	if(clipped_count == 0) return 0;

	// edge 2

	EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);

	clipped_count = PLANE_CLIP_POLYGON3D(
	edgeplane,temp_points1,clipped_count,clip_points);

	return clipped_count;


	/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
	GUINT i1 = (i0+1)%3;
	// edge 0
	btVector3 temp_points[MAX_TRI_CLIPPING];
	btVector3 temp_points1[MAX_TRI_CLIPPING];

	GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
	0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
	DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));


	if(clipped_count == 0) return 0;

	// edge 1
	clipped_count = PLANE_CLIP_POLYGON_GENERIC(
	0,temp_points,clipped_count,temp_points1,
	DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));

	if(clipped_count == 0) return 0;

	// edge 2
	clipped_count = PLANE_CLIP_POLYGON_GENERIC(
	0,temp_points1,clipped_count,clipped_points,
	DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));

	return clipped_count;*/
	}

	SIMD_FORCE_INLINE void sort_isect(
	GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
	{
	if(isect1<isect0)
	{
	//swap
	GIM_SWAP_NUMBERS(isect0,isect1);
	GIM_SWAP_NUMBERS(e0,e1);
	btVector3 tmp = vec0;
	vec0 = vec1;
	vec1 = tmp;
	}
	}

	//! Test verifying interval intersection with the direction between planes
	/*!
	\pre tv_plane and tu_plane must be set
	\post
	distances[2] is set with the distance
	closest_point_u, closest_point_v, edge_edge_dir are set too
	\return
	- 0: faces are paralele
	- 1: face U casts face V
	- 2: face V casts face U
	- 3: nearest edges
	*/
	SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
	{
	// Compute direction of intersection line
	edge_edge_dir = tu_plane.cross(tv_plane);
	GREAL Dlen;
	VEC_LENGTH(edge_edge_dir,Dlen);

	if(Dlen<0.0001)
	{
	return 0; //faces near paralele
	}

	edge_edge_dir*= 1/Dlen;//normalize


	// Compute interval for triangle 1
	GUINT tu_e0,tu_e1;//edge indices
	GREAL tu_scale_e0,tu_scale_e1;//edge scale
	if(!compute_intervals(du[0],du[1],du[2],
	du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;

	// Compute interval for triangle 2
	GUINT tv_e0,tv_e1;//edge indices
	GREAL tv_scale_e0,tv_scale_e1;//edge scale

	if(!compute_intervals(dv[0],dv[1],dv[2],
	dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;

	//proyected vertices
	btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
	btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);

	btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
	btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);

	//proyected intervals
	GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
	GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};

	sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
	sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);

	const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
	const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint

	if(midpoint_u<midpoint_v)
	{
	if(isect_u[1]>=isect_v[1]) // face U casts face V
	{
	return 1;
	}
	else if(isect_v[0]<=isect_u[0]) // face V casts face U
	{
	return 2;
	}
	// closest points
	closest_point_u = up_e1;
	closest_point_v = vp_e0;
	// calc edges and separation

	if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
	{
	SEGMENT_COLLISION(
	tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
	tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
	closest_point_u,
	closest_point_v);

	edge_edge_dir = closest_point_u-closest_point_v;
	VEC_LENGTH(edge_edge_dir,distances[2]);
	edge_edge_dir *= 1.0f/distances[2];// normalize
	}
	else
	{
	distances[2] = isect_v[0]-isect_u[1];//distance negative
	//edge_edge_dir *= -1.0f; //normal pointing from V to U
	}

	}
	else
	{
	if(isect_v[1]>=isect_u[1]) // face V casts face U
	{
	return 2;
	}
	else if(isect_u[0]<=isect_v[0]) // face U casts face V
	{
	return 1;
	}
	// closest points
	closest_point_u = up_e0;
	closest_point_v = vp_e1;
	// calc edges and separation

	if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
	{
	SEGMENT_COLLISION(
	tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
	tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
	closest_point_u,
	closest_point_v);

	edge_edge_dir = closest_point_u-closest_point_v;
	VEC_LENGTH(edge_edge_dir,distances[2]);
	edge_edge_dir *= 1.0f/distances[2];// normalize
	}
	else
	{
	distances[2] = isect_u[0]-isect_v[1];//distance negative
	//edge_edge_dir *= -1.0f; //normal pointing from V to U
	}
	}
	return 3;
	}


	//! collides by two sides
	SIMD_FORCE_INLINE bool triangle_collision(
	const btVector3 & u0,
	const btVector3 & u1,
	const btVector3 & u2,
	GREAL margin_u,
	const btVector3 & v0,
	const btVector3 & v1,
	const btVector3 & v2,
	GREAL margin_v,
	GIM_TRIANGLE_CONTACT_DATA & contacts)
	{

	margin = margin_u + margin_v;

	tu_vertices[0] = u0;
	tu_vertices[1] = u1;
	tu_vertices[2] = u2;

	tv_vertices[0] = v0;
	tv_vertices[1] = v1;
	tv_vertices[2] = v2;

	//create planes
	// plane v vs U points

	TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);

	du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
	du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
	du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);


	du0du1 = du[0] * du[1];
	du0du2 = du[0] * du[2];


	if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
	{
	if(du[0]<0) //we need test behind the triangle plane
	{
	distances[0] = GIM_MAX3(du[0],du[1],du[2]);
	distances[0] = -distances[0];
	if(distances[0]>margin) return false; //never intersect

	//reorder triangle v
	VEC_SWAP(tv_vertices[0],tv_vertices[1]);
	VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
	}
	else
	{
	distances[0] = GIM_MIN3(du[0],du[1],du[2]);
	if(distances[0]>margin) return false; //never intersect
	}
	}
	else
	{
	//Look if we need to invert the triangle
	distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid

	if(distances[0]<0.0f)
	{
	//reorder triangle v
	VEC_SWAP(tv_vertices[0],tv_vertices[1]);
	VEC_SCALE_4(tv_plane,-1.0f,tv_plane);

	distances[0] = GIM_MAX3(du[0],du[1],du[2]);
	distances[0] = -distances[0];
	}
	else
	{
	distances[0] = GIM_MIN3(du[0],du[1],du[2]);
	}
	}


	// plane U vs V points

	TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);

	dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
	dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
	dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);

	dv0dv1 = dv[0] * dv[1];
	dv0dv2 = dv[0] * dv[2];


	if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
	{
	if(dv[0]<0) //we need test behind the triangle plane
	{
	distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
	distances[1] = -distances[1];
	if(distances[1]>margin) return false; //never intersect

	//reorder triangle u
	VEC_SWAP(tu_vertices[0],tu_vertices[1]);
	VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
	}
	else
	{
	distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
	if(distances[1]>margin) return false; //never intersect
	}
	}
	else
	{
	//Look if we need to invert the triangle
	distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid

	if(distances[1]<0.0f)
	{
	//reorder triangle v
	VEC_SWAP(tu_vertices[0],tu_vertices[1]);
	VEC_SCALE_4(tu_plane,-1.0f,tu_plane);

	distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
	distances[1] = -distances[1];
	}
	else
	{
	distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
	}
	}

	GUINT bl;
	/* bl = cross_line_intersection_test();
	if(bl==3)
	{
	//take edge direction too
	bl = distances.maxAxis();
	}
	else
	{*/
	bl = 0;
	if(distances[0]<distances[1]) bl = 1;
	//}

	if(bl==2) //edge edge separation
	{
	if(distances[2]>margin) return false;

	contacts.m_penetration_depth = -distances[2] + margin;
	contacts.m_points[0] = closest_point_v;
	contacts.m_point_count = 1;
	VEC_COPY(contacts.m_separating_normal,edge_edge_dir);

	return true;
	}

	//clip face against other


	GUINT point_count;
	//TODO
	if(bl == 0) //clip U points against V
	{
	point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
	if(point_count == 0) return false;
	contacts.merge_points(tv_plane,margin,contact_points,point_count);
	}
	else //clip V points against U
	{
	point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
	if(point_count == 0) return false;
	contacts.merge_points(tu_plane,margin,contact_points,point_count);
	contacts.m_separating_normal *= -1.f;
	}
	if(contacts.m_point_count == 0) return false;
	return true;
	}

	};


	/*class GIM_TRIANGLE_CALCULATION_CACHE
	{
	public:
	GREAL margin;
	GUINT clipped_count;
	btVector3 tu_vertices[3];
	btVector3 tv_vertices[3];
	btVector3 temp_points[MAX_TRI_CLIPPING];
	btVector3 temp_points1[MAX_TRI_CLIPPING];
	btVector3 clipped_points[MAX_TRI_CLIPPING];
	GIM_TRIANGLE_CONTACT_DATA contacts1;
	GIM_TRIANGLE_CONTACT_DATA contacts2;


	//! clip triangle
	GUINT clip_triangle(
	const btVector4 & tri_plane,
	const btVector3 * tripoints,
	const btVector3 * srcpoints,
	btVector3 * clipped_points)
	{
	// edge 0

	btVector4 edgeplane;

	EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);

	GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
	edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);

	if(clipped_count == 0) return 0;

	// edge 1

	EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);

	clipped_count = PLANE_CLIP_POLYGON3D(
	edgeplane,temp_points,clipped_count,temp_points1);

	if(clipped_count == 0) return 0;

	// edge 2

	EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);

	clipped_count = PLANE_CLIP_POLYGON3D(
	edgeplane,temp_points1,clipped_count,clipped_points);

	return clipped_count;
	}




	//! collides only on one side
	bool triangle_collision(
	const btVector3 & u0,
	const btVector3 & u1,
	const btVector3 & u2,
	GREAL margin_u,
	const btVector3 & v0,
	const btVector3 & v1,
	const btVector3 & v2,
	GREAL margin_v,
	GIM_TRIANGLE_CONTACT_DATA & contacts)
	{

	margin = margin_u + margin_v;


	tu_vertices[0] = u0;
	tu_vertices[1] = u1;
	tu_vertices[2] = u2;

	tv_vertices[0] = v0;
	tv_vertices[1] = v1;
	tv_vertices[2] = v2;

	//create planes
	// plane v vs U points


	TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);

	clipped_count = clip_triangle(
	contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);

	if(clipped_count == 0 )
	{
	return false;//Reject
	}

	//find most deep interval face1
	contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
	if(contacts1.m_point_count == 0) return false; // too far

	//Normal pointing to triangle1
	//contacts1.m_separating_normal *= -1.f;

	//Clip tri1 by tri2 edges

	TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);

	clipped_count = clip_triangle(
	contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);

	if(clipped_count == 0 )
	{
	return false;//Reject
	}

	//find most deep interval face1
	contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
	if(contacts2.m_point_count == 0) return false; // too far

	contacts2.m_separating_normal *= -1.f;

	////check most dir for contacts
	if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
	{
	contacts.copy_from(contacts2);
	}
	else
	{
	contacts.copy_from(contacts1);
	}
	return true;
	}


	};*/



	bool GIM_TRIANGLE::collide_triangle_hard_test(
	const GIM_TRIANGLE & other,
	GIM_TRIANGLE_CONTACT_DATA & contact_data) const
	{
	GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
	return calc_cache.triangle_collision(
	m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
	other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
	contact_data);

	}