upgrade to GIMPACT 0.2

Extras/GIMPACT/src/core/gim_box_set.cpp

@@ -0,0 +1,182 @@
+
+/*
+-----------------------------------------------------------------------------
+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 "GIMPACT/core/gim_box_set.h"
+
+
+GUINT GIM_BOX_TREE::_calc_splitting_axis(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	GUINT numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
+	GUINT endIndex, GUINT splitAxis)
+{
+	GUINT i;
+	GUINT splitIndex =startIndex;
+	GUINT numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+	for (i=startIndex;i<endIndex;i++)
+	{
+		splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+	}
+	splitValue /= (btScalar)numIndices;
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+		if (center > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	GUINT rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+}
+
+
+void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT current_index = m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if((endIndex-startIndex) == 1) //we got a leaf
+	{		
+		m_node_array[current_index].m_left = 0;
+		m_node_array[current_index].m_right = 0;
+		m_node_array[current_index].m_escapeIndex = 0;
+
+		m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound;
+		m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data;
+		return;
+	}
+
+	//configure inner node
+
+	GUINT splitIndex;
+
+	//calc this node bounding box
+	m_node_array[current_index].m_bound.invalidate();	
+	for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
+	{
+		m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
+	}
+
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,splitIndex);
+
+	//configure this inner node : the left node index
+	m_node_array[current_index].m_left = m_num_nodes;
+	//build left child tree
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+	//configure this inner node : the right node index
+	m_node_array[current_index].m_right = m_num_nodes;
+
+	//build right child tree
+	_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	//configure this inner node : the escape index
+	m_node_array[current_index].m_escapeIndex  = m_num_nodes - current_index;
+}
+
+//! stackless build tree
+void GIM_BOX_TREE::build_tree(
+	gim_array<GIM_AABB_DATA> & primitive_boxes)
+{
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+	
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+

Extras/GIMPACT/src/core/gim_contact.cpp

@@ -0,0 +1,142 @@
+
+/*
+-----------------------------------------------------------------------------
+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 "GIMPACT/core/gim_contact.h"
+
+#define MAX_COINCIDENT 8
+
+void gim_contact_array::merge_contacts(
+	const gim_contact_array & contacts, bool normal_contact_average)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
+	keycontacts.resize(contacts.size(),false);
+
+	//fill key contacts
+
+	for (GUINT i = 0;i<contacts.size() ;i++ )
+	{
+		keycontacts[i].m_key = contacts[i].calc_key_contact();
+		keycontacts[i].m_value = i;
+	}
+
+	//sort keys
+	gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
+
+	// Merge contacts
+
+	GUINT coincident_count=0;
+	btVector3 coincident_normals[MAX_COINCIDENT];
+
+	GUINT last_key = keycontacts[0].m_key;
+	GUINT key = 0;
+
+	push_back(contacts[keycontacts[0].m_value]);
+	GIM_CONTACT * pcontact = &back();
+
+	for(GUINT i=1;i<keycontacts.size();i++)
+	{
+	    key = keycontacts[i].m_key;
+		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+		if(last_key ==  key)//same points
+		{
+			//merge contact
+			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			{
+				*pcontact = *scontact;
+                coincident_count = 0;
+			}
+			else if(normal_contact_average)
+			{
+				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+                {
+                    if(coincident_count<MAX_COINCIDENT)
+                    {
+                    	coincident_normals[coincident_count] = scontact->m_normal;
+                        coincident_count++;
+                    }
+                }
+			}
+		}
+		else
+		{//add new contact
+
+		    if(normal_contact_average && coincident_count>0)
+		    {
+		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
+		        coincident_count = 0;
+		    }
+
+		    push_back(*scontact);
+		    pcontact = &back();
+        }
+		last_key = key;
+	}
+}
+
+void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	GIM_CONTACT average_contact = contacts.back();
+
+	for (GUINT i=1;i<contacts.size() ;i++ )
+	{
+		average_contact.m_point += contacts[i].m_point;
+		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+	}
+
+	//divide
+	GREAL divide_average = 1.0f/((GREAL)contacts.size());
+
+	average_contact.m_point *= divide_average;
+
+	average_contact.m_normal *= divide_average;
+
+	average_contact.m_depth = average_contact.m_normal.length();
+
+	average_contact.m_normal /= average_contact.m_depth;
+
+}
+

Extras/GIMPACT/src/core/gim_memory.cpp

@@ -0,0 +1,135 @@
+/*
+-----------------------------------------------------------------------------
+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 "GIMPACT/core/gim_memory.h"
+#include "stdlib.h"
+
+#ifdef GIM_SIMD_MEMORY
+#include "LinearMath/btAlignedAllocator.h"
+#endif
+
+static gim_alloc_function *g_allocfn = 0;
+static gim_alloca_function *g_allocafn = 0;
+static gim_realloc_function *g_reallocfn = 0;
+static gim_free_function *g_freefn = 0;
+
+void gim_set_alloc_handler (gim_alloc_function *fn)
+{
+  g_allocfn = fn;
+}
+
+void gim_set_alloca_handler (gim_alloca_function *fn)
+{
+  g_allocafn = fn;
+}
+
+void gim_set_realloc_handler (gim_realloc_function *fn)
+{
+  g_reallocfn = fn;
+}
+
+void gim_set_free_handler (gim_free_function *fn)
+{
+  g_freefn = fn;
+}
+
+gim_alloc_function *gim_get_alloc_handler()
+{
+  return g_allocfn;
+}
+
+gim_alloca_function *gim_get_alloca_handler()
+{
+  return g_allocafn;
+}
+
+
+gim_realloc_function *gim_get_realloc_handler ()
+{
+  return g_reallocfn;
+}
+
+
+gim_free_function  *gim_get_free_handler ()
+{
+  return g_freefn;
+}
+
+
+void * gim_alloc(size_t size)
+{
+	void * ptr;
+	if (g_allocfn)
+	{
+		ptr = g_allocfn(size);
+	}
+	else
+	{
+#ifdef GIM_SIMD_MEMORY
+		ptr = btAlignedAlloc(size,16);
+#else
+		ptr = malloc(size);
+#endif
+	}
+  	return ptr;
+}
+
+void * gim_alloca(size_t size)
+{
+  if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
+}
+
+
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
+{
+ 	void * newptr = gim_alloc(newsize);
+    size_t copysize = oldsize<newsize?oldsize:newsize;
+    gim_simd_memcpy(newptr,ptr,copysize);
+    gim_free(ptr);
+    return newptr;
+}
+
+void gim_free(void *ptr)
+{
+	if (!ptr) return;
+	if (g_freefn)
+	{
+	   g_freefn(ptr);
+	}
+	else
+	{
+	#ifdef GIM_SIMD_MEMORY
+		btAlignedFree(ptr);
+	#else
+		free(ptr);
+	#endif
+	}
+}
+

Extras/GIMPACT/src/core/gim_tri_collision.cpp

@@ -0,0 +1,640 @@
+
+/*! \file gim_tri_collision.h
+\author Francisco Le<4C>n 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 "GIMPACT/core/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);
+
+}
+
+
+
+