/*!
\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 dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
dVector3 dv[3];
gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]);
GetPointFromBarycentric(dv, u, v, Out);
gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
}
/*!
In each contact
<ul>
<li> m_handle1 points to trimesh.
<li> m_handle2 points to NULL.
<li> m_feature1 Is a triangle index of trimesh.
</ul>
\param trimesh
\param center
\param radius
\param contacts A GIM_CONTACT array. Must be initialized
*/
void gim_trimesh_sphere_collision(GIM_TRIMESH * trimesh,vec3f center,GREAL radius, GDYNAMIC_ARRAY * contacts)
{
contacts->m_size = 0;
aabb3f test_aabb;
test_aabb.minX = center[0]-radius;
test_aabb.maxX = center[0]+radius;
test_aabb.minY = center[1]-radius;
test_aabb.maxY = center[1]+radius;
test_aabb.minZ = center[2]-radius;
test_aabb.maxZ = center[2]+radius;
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_BOXQUERY_LIST(collision_result);
gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
if(collision_result.m_size==0)
{
GIM_DYNARRAY_DESTROY(collision_result);
}
//collide triangles
//Locks trimesh
gim_trimesh_locks_work_data(trimesh);
//dummy contacts
GDYNAMIC_ARRAY dummycontacts;
GIM_CREATE_CONTACT_LIST(dummycontacts);
int cresult;
unsigned int i;
GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
GIM_TRIANGLE_CONTACT_DATA tri_contact_data;
GIM_TRIANGLE_DATA tri_data;
for(i=0;i<collision_result.m_size;i++)
{
gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
cresult = gim_triangle_sphere_collision(&tri_data,center,radius,&tri_contact_data);
if(cresult!=0)
{
GIM_PUSH_CONTACT(dummycontacts, tri_contact_data.m_points[0],tri_contact_data.m_separating_normal ,tri_contact_data.m_penetration_depth,trimesh, 0, boxesresult[i],0);
}
}
///unlocks
gim_trimesh_unlocks_work_data(trimesh);
///Destroy box result
GIM_DYNARRAY_DESTROY(collision_result);
//merge contacts
gim_merge_contacts(&dummycontacts,contacts);
//Destroy dummy
GIM_DYNARRAY_DESTROY(dummycontacts);
}
/*!
\post If gim_trimesh_needs_update returns 1, then it calls gim_trimesh_update_vertices and gim_trimesh_update_aabbset
*/
void gim_trimesh_update(GIM_TRIMESH * trimesh)
{
if(gim_trimesh_needs_update(trimesh)==0) return;
gim_trimesh_update_vertices(trimesh);
gim_trimesh_locks_work_data(trimesh);
gim_trimesh_update_aabbset(trimesh);
gim_trimesh_unlocks_work_data(trimesh);
//Clear update flag
trimesh->m_mask &= ~GIM_TRIMESH_NEED_UPDATE;
}
/*!
\param trimesh
\param contact
\return 1 if the ray collides, else 0
*/
int gim_trimesh_ray_collision(GIM_TRIMESH * trimesh,vec3f origin,vec3f dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA * contact)
{
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_BOXQUERY_LIST(collision_result);
gim_aabbset_ray_collision(origin,dir,tmax,&trimesh->m_aabbset,&collision_result);
if(collision_result.m_size==0)
{
GIM_DYNARRAY_DESTROY(collision_result);
return 0;
}
//collide triangles
GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
GIM_TRIANGLE_DATA tridata;
vec3f pout;
GREAL tparam,u,v;
char does_intersect;
gim_trimesh_locks_work_data(trimesh);
for(unsigned int i=0;i<collision_result.m_size;i++)
{
gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tridata);
RAY_TRIANGLE_INTERSECTION(origin,dir,tridata.m_vertices[0],tridata.m_vertices[1],tridata.m_vertices[2],tridata.m_planes.m_planes[0],pout,u,v,tparam,tmax,does_intersect);
if(does_intersect)
{
contact->tparam = tparam;
contact->u = u;
contact->v = v;
contact->m_face_id = boxesresult[i];
VEC_COPY(contact->m_point,pout);
VEC_COPY(contact->m_normal,tridata.m_planes.m_planes[0]);
gim_trimesh_unlocks_work_data(trimesh);
GIM_DYNARRAY_DESTROY(collision_result);
return 1;
}
}
gim_trimesh_unlocks_work_data(trimesh);
GIM_DYNARRAY_DESTROY(collision_result);
return 0;//no collisiion
}
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dCylinderClass );
dIASSERT( o2->type == dTriMeshClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Main data holder
sData cData;
// Assign ODE stuff
cData.gCylinder = o1;
cData.gTrimesh = (dxTriMesh*)o2;
cData.iFlags = flags;
cData.iSkip = skip;
cData.gContact = contact;
cData.nContacts = 0;
_InitCylinderTrimeshData(cData);
//*****at first , collide box aabb******//
aabb3f test_aabb;
test_aabb.minX = o1->aabb[0];
test_aabb.maxX = o1->aabb[1];
test_aabb.minY = o1->aabb[2];
test_aabb.maxY = o1->aabb[3];
test_aabb.minZ = o1->aabb[4];
test_aabb.maxZ = o1->aabb[5];
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_BOXQUERY_LIST(collision_result);
gim_aabbset_box_collision(&test_aabb, &cData.gTrimesh->m_collision_trimesh.m_aabbset , &collision_result);
if(collision_result.m_size==0)
{
GIM_DYNARRAY_DESTROY(collision_result);
return 0;
}
//*****Set globals for box collision******//
int ctContacts0 = 0;
cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh;
gim_trimesh_locks_work_data(ptrimesh);
for(unsigned int i=0; i<collision_result.m_size; i++)
{
const int Triint = boxesresult[i];
dVector3 dv[3];
gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
// test this triangle
TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
// fill-in triangle index for generated contacts
for (; ctContacts0<cData.nContacts; ctContacts0++)
cData.gLocalContacts[ctContacts0].triIndex = Triint;
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
{
break;
}
}
gim_trimesh_unlocks_work_data(ptrimesh);
GIM_DYNARRAY_DESTROY(collision_result);
return _ProcessLocalContacts(cData);
}
/*!
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);
}
/*!
Find the closest primitive collided by the ray
\param trimesh
\param capsule
\param contact
\param contacts A GIM_CONTACT array. Must be initialized
*/
void gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
{
contacts->m_size = 0;
aabb3f test_aabb;
CALC_CAPSULE_AABB((*capsule),test_aabb);
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_BOXQUERY_LIST(collision_result);
gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
if(collision_result.m_size==0)
{
GIM_DYNARRAY_DESTROY(collision_result);
}
//collide triangles
//Locks trimesh
gim_trimesh_locks_work_data(trimesh);
//dummy contacts
GDYNAMIC_ARRAY dummycontacts;
GIM_CREATE_CONTACT_LIST(dummycontacts);
int cresult;
unsigned int i;
GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
GIM_TRIANGLE_DATA tri_data;
GUINT old_contact_size;
GIM_CONTACT * pcontact;
for(i=0;i<collision_result.m_size;i++)
{
old_contact_size = dummycontacts.m_size;
gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
cresult = gim_triangle_capsule_collision(&tri_data, capsule, &dummycontacts);
if(cresult!=0)
{
pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,dummycontacts);
pcontact+= old_contact_size;
while(old_contact_size<dummycontacts.m_size)
{
pcontact->m_handle1 = trimesh;
pcontact->m_handle2 = capsule;
pcontact->m_feature1 = boxesresult[i];
pcontact->m_feature2 = 0;
pcontact++;
old_contact_size++;
}
}
}
///unlocks
gim_trimesh_unlocks_work_data(trimesh);
///Destroy box result
GIM_DYNARRAY_DESTROY(collision_result);
//merge contacts
gim_merge_contacts(&dummycontacts,contacts);
//Destroy dummy
GIM_DYNARRAY_DESTROY(dummycontacts);
}
