349 lines
10 KiB
C++
Executable File
349 lines
10 KiB
C++
Executable File
|
|
/*
|
|
-----------------------------------------------------------------------------
|
|
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;\
|
|
GUINT _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
|
|
GUINT _max_candidates[MAX_TRI_CLIPPING];
|
|
vec3f _temp_clip[MAX_TRI_CLIPPING];
|
|
GUINT _temp_clip_count = 0;
|
|
vec3f _temp_clip2[MAX_TRI_CLIPPING];
|
|
GUINT _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
|
|
GUINT mostdir=0;
|
|
GUINT 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
|
|
GUINT 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
|
|
|
|
GUINT 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
|
|
GUINT 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;
|
|
|
|
|
|
GUINT 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
|
|
GUINT 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);
|
|
}
|