/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

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

Copyright (c) 2006 Francisco Leon. 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.

 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 and GIMPACT-LICENSE-BSD.TXT for more details.

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

#include "GIMPACT/gim_trimesh.h"

#define CLASSIFY_TRI_BY_FACE(v1,v2,v3,faceplane,out_of_face)\
{   \
    _distances[0] = DISTANCE_PLANE_POINT(faceplane,v1);\
    _distances[1] =  _distances[0] * DISTANCE_PLANE_POINT(faceplane,v2);\
    _distances[2] =  _distances[0] * DISTANCE_PLANE_POINT(faceplane,v3); \
	if(_distances[1]>0.0f && _distances[2]>0.0f)\
	{\
	    out_of_face = 1;\
	}\
	else\
	{\
	    out_of_face = 0;\
	}\
}\


//! Receives the 3 edge planes
#define MOST_DEEP_POINTS(plane,points,point_count,deep_points,deep_points_count,maxdeep)\
{\
    maxdeep=-1000.0f;\
    GUINT32 _k;\
	GREAL _dist;\
	deep_points_count = 0;\
	for(_k=0;_k<point_count;_k++)\
	{\
	    _dist = -DISTANCE_PLANE_POINT(plane,points[_k]);\
		if(_dist>maxdeep)\
		{\
			maxdeep = _dist;\
			_max_candidates[0] = _k;\
			deep_points_count=1;\
		}\
		else if((_dist+G_EPSILON)>=maxdeep)\
		{\
		    _max_candidates[deep_points_count] = _k;\
			deep_points_count++;\
		}\
	}\
	if(maxdeep<0.0f)\
    {\
        deep_points_count = 0;\
    }\
    else\
    {\
        for(_k=0;_k<deep_points_count;_k++)\
        {\
            VEC_COPY(deep_points[_k],points[_max_candidates[_k]]);\
        }\
    }\
}\

//! Receives the 3 edge planes
#define CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri_points,tri_edge_planes, clipped_points, clipped_point_count)\
{\
    clipped_point_count = 0;    \
    _temp_clip_count = 0;\
    PLANE_CLIP_POLYGON(tri_edge_planes[0],tri_points,3,_temp_clip,_temp_clip_count,MAX_TRI_CLIPPING);\
    if(_temp_clip_count>0)\
    {\
        _temp_clip_count2 = 0;\
        PLANE_CLIP_POLYGON(tri_edge_planes[1],_temp_clip,_temp_clip_count,_temp_clip2,_temp_clip_count2,MAX_TRI_CLIPPING);\
        if(_temp_clip_count2>0)\
        {\
            PLANE_CLIP_POLYGON(tri_edge_planes[2],_temp_clip2,_temp_clip_count2,clipped_points,clipped_point_count,MAX_TRI_CLIPPING);\
        }\
    }\
}\



int _gim_triangle_triangle_collision(
							GIM_TRIANGLE_DATA *tri1,
							GIM_TRIANGLE_DATA *tri2,
							GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
    //Cache variables for triangle intersection
    GUINT32 _max_candidates[MAX_TRI_CLIPPING];
    vec3f _temp_clip[MAX_TRI_CLIPPING];
    GUINT32 _temp_clip_count = 0;
    vec3f _temp_clip2[MAX_TRI_CLIPPING];
    GUINT32 _temp_clip_count2 = 0;
    vec3f clipped_points2[MAX_TRI_CLIPPING];
    vec3f deep_points2[MAX_TRI_CLIPPING];
    vec3f clipped_points1[MAX_TRI_CLIPPING];
    vec3f deep_points1[MAX_TRI_CLIPPING];



    //State variabnles
	GUINT32 mostdir=0;
	GUINT32 clipped2_count=0;

	//Clip tri2 by tri1 edges

	CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri2->m_vertices,(&tri1->m_planes.m_planes[1]), clipped_points2, clipped2_count);

	if(clipped2_count == 0 )
	{
	     return 0;//Reject
	}

	//find most deep interval face1
	GUINT32 deep2_count=0;

	GREAL maxdeep;

	MOST_DEEP_POINTS((tri1->m_planes.m_planes[0]), clipped_points2, clipped2_count, deep_points2, deep2_count, maxdeep);
	if(deep2_count==0)
	{
//	    *perror = 0.0f;
	     return 0;//Reject
	}

	//Normal pointing to triangle1
	VEC_SCALE(contact_data->m_separating_normal,-1.0f,(tri1->m_planes.m_planes[0]));


	//Clip tri1 by tri2 edges

	GUINT32 clipped1_count=0;

	CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri1->m_vertices,(&tri2->m_planes.m_planes[1]), clipped_points1, clipped1_count);

	if(clipped2_count == 0 )
	{
//	    *perror = 0.0f;
	     return 0;//Reject
	}


	//find interval face2
	GUINT32 deep1_count=0;

	GREAL dist;

	MOST_DEEP_POINTS((tri2->m_planes.m_planes[0]), clipped_points1, clipped1_count, deep_points1, deep1_count, dist);

	if(deep1_count==0)
	{
//	    *perror = 0.0f;
	    return 0;
	}

	if(dist<maxdeep)
	{
		maxdeep = dist;
		mostdir = 1;
		VEC_COPY(contact_data->m_separating_normal,(tri2->m_planes.m_planes[0]));
	}
	//set deep
	contact_data->m_penetration_depth = maxdeep;

	////check most dir for contacts
	if(mostdir==0)
	{
	    contact_data->m_point_count = deep2_count;
	    for(mostdir=0;mostdir<deep2_count;mostdir++)
	    {
	        VEC_COPY(contact_data->m_points[mostdir] ,deep_points2[mostdir]);
	    }
	}
	else
	{
		contact_data->m_point_count = deep1_count;
	    for(mostdir=0;mostdir<deep1_count;mostdir++)
	    {
	        VEC_COPY(contact_data->m_points[mostdir] ,deep_points1[mostdir]);
	    }
	}
	return 1;
}



//! Finds the contact points from a collision of two triangles
/*!
Returns the contact points, the penetration depth and the separating normal of the collision
between two triangles. The normal is pointing toward triangle 1 from triangle 2
*/
int gim_triangle_triangle_collision(
							GIM_TRIANGLE_DATA *tri1,
							GIM_TRIANGLE_DATA *tri2,
							GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
    vec3f _distances;
    char out_of_face=0;

    CLASSIFY_TRI_BY_FACE(tri1->m_vertices[0],tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_planes.m_planes[0],out_of_face);
    if(out_of_face==1) return 0;

    CLASSIFY_TRI_BY_FACE(tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2],tri1->m_planes.m_planes[0],out_of_face);
    if(out_of_face==1) return 0;

    return _gim_triangle_triangle_collision(tri1,tri2,contact_data);
}

//! Trimesh Trimesh Collisions
/*!

In each contact
<ul>
<li> m_handle1 points to trimesh1.
<li> m_handle2 points to trimesh2.
<li> m_feature1 Is a triangle index of trimesh1.
<li> m_feature2 Is a triangle index of trimesh2.
</ul>

\param trimesh1 Collider
\param trimesh2 Collidee
\param contacts A GIM_CONTACT array. Must be initialized
*/
void gim_trimesh_trimesh_collision(GIM_TRIMESH * trimesh1, GIM_TRIMESH * trimesh2, GDYNAMIC_ARRAY * contacts)
{
    contacts->m_size = 0;
    GDYNAMIC_ARRAY collision_pairs;
    GIM_CREATE_PAIR_SET(collision_pairs)

    gim_aabbset_bipartite_intersections(&trimesh1->m_aabbset,&trimesh2->m_aabbset,&collision_pairs);

    if(collision_pairs.m_size==0)
    {
        GIM_DYNARRAY_DESTROY(collision_pairs);
         return; //no collisioin
    }

    //Locks meshes
    gim_trimesh_locks_work_data(trimesh1);
    gim_trimesh_locks_work_data(trimesh2);


    //pair pointer
    GIM_PAIR *pairs = GIM_DYNARRAY_POINTER(GIM_PAIR,collision_pairs);
    //dummy contacts
    GDYNAMIC_ARRAY dummycontacts;
    GIM_CREATE_CONTACT_LIST(dummycontacts);

    //Auxiliary triangle data
    GIM_TRIANGLE_CONTACT_DATA tri_contact_data;
    GIM_TRIANGLE_DATA tri1data,tri2data;


    GUINT32 i, ti1,ti2,ci;
    int colresult;
    for (i=0;i<collision_pairs.m_size; i++)
    {
        ti1 = pairs[i].m_index1;
        ti2 = pairs[i].m_index2;
        //Get triangles data
        gim_trimesh_get_triangle_data(trimesh1,ti1,&tri1data);
        gim_trimesh_get_triangle_data(trimesh2,ti2,&tri2data);

        //collide triangles
        colresult = gim_triangle_triangle_collision(&tri1data,&tri2data,&tri_contact_data);
        if(colresult == 1)
        {
            //Add contacts
            for (ci=0;ci<tri_contact_data.m_point_count ;ci++ )
            {
                GIM_PUSH_CONTACT(dummycontacts, tri_contact_data.m_points[ci],tri_contact_data.m_separating_normal ,tri_contact_data.m_penetration_depth,trimesh1, trimesh2, ti1, ti2);
            }
        }
    }

    if(dummycontacts.m_size == 0) //reject
    {
        GIM_DYNARRAY_DESTROY(dummycontacts);
        GIM_DYNARRAY_DESTROY(collision_pairs);
        return;
    }
    //merge contacts
    gim_merge_contacts(&dummycontacts,contacts);

    //Terminate
    GIM_DYNARRAY_DESTROY(dummycontacts);
    GIM_DYNARRAY_DESTROY(collision_pairs);

    //Unlocks meshes
    gim_trimesh_unlocks_work_data(trimesh1);
    gim_trimesh_unlocks_work_data(trimesh2);
}


//! Trimesh Plane Collisions
/*!

\param trimesh
\param plane vec4f plane
\param contacts A vec4f array. Must be initialized (~100). Each element have the coordinate point in the first 3 elements, and vec4f[3] has the penetration depth.
*/
void gim_trimesh_plane_collision(GIM_TRIMESH * trimesh,vec4f plane, GDYNAMIC_ARRAY * contacts)
{
    contacts->m_size = 0;
    char classify;
    PLANE_CLASSIFY_BOX(plane,trimesh->m_aabbset.m_global_bound,classify);
    if(classify>1) return; // in front of plane

    //Locks mesh
    gim_trimesh_locks_work_data(trimesh);
    //Get vertices
    GUINT32 i, vertcount = trimesh->m_transformed_vertex_buffer.m_element_count;
    vec3f * vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);

    GREAL dist;
    vec4f * result_contact;

    for (i=0;i<vertcount;i++)
    {
        dist = DISTANCE_PLANE_POINT(plane,vertices[i]);
        if(dist<=0.0f)
        {
             GIM_DYNARRAY_PUSH_EMPTY(vec4f,(*contacts));
             result_contact = GIM_DYNARRAY_POINTER_LAST(vec4f,(*contacts));
             VEC_COPY((*result_contact),vertices[i]);
             (*result_contact)[3] = -dist;
        }
    }
    gim_trimesh_unlocks_work_data(trimesh);
}