/*!

\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);
}
void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
					GDYNAMIC_ARRAY * dest_contacts)
{
    dest_contacts->m_size = 0;
    //Traverse the source contacts
	GUINT32 source_count = source_contacts->m_size;
	if(source_count==0) return;

	GIM_CONTACT * psource_contacts	= GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));

	//add the unique contact
	GIM_CONTACT * pcontact = 0;
    GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
    pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
    //set the first contact
    GIM_COPY_CONTACTS(pcontact, psource_contacts);

    if(source_count==1) return;
    //scale the first contact
    VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);

    psource_contacts++;

	//Average the contacts
    GUINT32 i;
	for(i=1;i<source_count;i++)
	{
	    VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
	    VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
	    psource_contacts++;
	}

	GREAL divide_average = 1.0f/((GREAL)source_count);

	VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);

	pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
	GIM_SQRT(pcontact->m_depth,pcontact->m_depth);

	VEC_NORMALIZE(pcontact->m_normal);

	/*GREAL normal_len;
    VEC_INV_LENGTH(pcontact->m_normal,normal_len);
	VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);

    //Deep = LEN(normal)/SQRT(source_count)
    GIM_SQRT(divide_average,divide_average);
	pcontact->m_depth = divide_average/normal_len;
	*/
}
void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
					GDYNAMIC_ARRAY * dest_contacts)
{
    dest_contacts->m_size = 0;

	GUINT32 source_count = source_contacts->m_size;
	GIM_CONTACT * psource_contacts	= GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
	//create keys
	GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);

    GUINT32 i;
	for(i=0;i<source_count;i++)
	{
		keycontacts[i].m_value = i;
		GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
	}

	//sort keys
	GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);

	// Merge contacts
	GIM_CONTACT * pcontact = 0;
	GIM_CONTACT * scontact = 0;
	GUINT32 key,last_key=0;

	for(i=0;i<source_contacts->m_size;i++)
	{
	    key = keycontacts[i].m_key;
		scontact = &psource_contacts[keycontacts[i].m_value];

		if(i>0 && last_key ==  key)
		{
			//merge contact
			if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
			{
			    GIM_COPY_CONTACTS(pcontact, scontact);
			}
		}
		else
		{//add new contact
		    GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
            pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
		    GIM_COPY_CONTACTS(pcontact, scontact);
        }
		last_key = key;
	}
	gim_free(keycontacts,0);
}