void gim_destroy_buffer_manager(GUINT buffer_manager_id)
{
GBUFFER_MANAGER_DATA * bm_data;
gim_get_buffer_manager_data(buffer_manager_id,&bm_data);
if(bm_data == 0) return;
//Destroy all buffers
GBUFFER_DATA * buffers = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
GUINT i, buffer_count = bm_data->m_buffer_array.m_size;
for (i=0;i<buffer_count ;i++ )
{
if(buffers[i].m_buffer_handle!=0) //Is active
{
// free handle
bm_data->m_prototype.free_fn(buffers[i].m_buffer_handle,buffers[i].m_size);
}
}
//destroy buffer array
GIM_DYNARRAY_DESTROY(bm_data->m_buffer_array);
//destroy free positions
GIM_DYNARRAY_DESTROY(bm_data->m_free_positions);
//Mark as innactive
bm_data->m_active = 0;
}
/*!
\param triangle
\param capsule
\param contacts Contains the closest points on the capsule, and the normal points to triangle
\post The contacts array is not set to 0. It adds aditional contacts
*/
int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
{
GUINT old_contact_size = contacts->m_size;
gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
pcontact+= old_contact_size;
if(pcontact->m_depth > capsule->m_radius)
{
contacts->m_size = old_contact_size;
return 0;
}
vec3f vec;
while(old_contact_size<contacts->m_size)
{
//Scale the normal for pointing to triangle
VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
//Fix the contact point
VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
//Fix the depth
pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
pcontact++;
old_contact_size++;
}
return 1;
}
/*!
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);
}
void GET_AVALIABLE_BUFFER_ID(GBUFFER_MANAGER_DATA * buffer_manager, GUINT32 & buffer_id)
{
if(buffer_manager->m_free_positions.m_size>0)\
{
GUINT32 * _pointer = GIM_DYNARRAY_POINTER(GUINT32,buffer_manager->m_free_positions);
buffer_id = _pointer[buffer_manager->m_free_positions.m_size-1];
GIM_DYNARRAY_POP_ITEM(buffer_manager->m_free_positions);
}
else
{
buffer_id = buffer_manager->m_buffer_array.m_size;
GIM_DYNARRAY_PUSH_EMPTY(GBUFFER_DATA,buffer_manager->m_buffer_array);
}
}
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);
}
/*!
\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
}
void gim_trimesh_get_triangle_data(GIM_TRIMESH * trimesh, GUINT32 triangle_index, GIM_TRIANGLE_DATA * tri_data)
{
vec3f * transformed_vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
GUINT32 * triangle_indices = GIM_BUFFER_ARRAY_POINTER(GUINT32,trimesh->m_tri_index_buffer,triangle_index*3);
//Copy the vertices
VEC_COPY(tri_data->m_vertices[0],transformed_vertices[triangle_indices[0]]);
VEC_COPY(tri_data->m_vertices[1],transformed_vertices[triangle_indices[1]]);
VEC_COPY(tri_data->m_vertices[2],transformed_vertices[triangle_indices[2]]);
//Get the planes
GIM_TRIPLANES_CACHE * planes = GIM_DYNARRAY_POINTER(GIM_TRIPLANES_CACHE,trimesh->m_planes_cache_buffer);
planes += triangle_index;
//verify planes cache
GUINT32 bit_eval;
GIM_BITSET_GET(trimesh->m_planes_cache_bitset,triangle_index,bit_eval);
if(bit_eval == 0)// Needs to calc the planes
{
//Calc the face plane
TRIANGLE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],tri_data->m_vertices[2],planes->m_planes[0]);
//Calc the edge 1
EDGE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],(planes->m_planes[0]),(planes->m_planes[1]));
//Calc the edge 2
EDGE_PLANE(tri_data->m_vertices[1],tri_data->m_vertices[2],(planes->m_planes[0]),(planes->m_planes[2]));
//Calc the edge 3
EDGE_PLANE(tri_data->m_vertices[2],tri_data->m_vertices[0],(planes->m_planes[0]), (planes->m_planes[3]));
//mark
GIM_BITSET_SET(trimesh->m_planes_cache_bitset,triangle_index);
}
VEC_COPY_4((tri_data->m_planes.m_planes[0]),(planes->m_planes[0]));//face plane
VEC_COPY_4((tri_data->m_planes.m_planes[1]),(planes->m_planes[1]));//edge1
VEC_COPY_4((tri_data->m_planes.m_planes[2]),(planes->m_planes[2]));//edge2
VEC_COPY_4((tri_data->m_planes.m_planes[3]),(planes->m_planes[3]));//edge3
}
GUINT32 gim_create_buffer_from_data(
GBUFFER_MANAGER_DATA buffer_managers[],
GUINT32 buffer_manager_id,
const void * pdata,
GUINT32 buffer_size,
int usage,
GBUFFER_ID * buffer_id)
{
VALIDATE_BUFFER_MANAGER(buffer_managers,buffer_manager_id)
GPTR newbufferhandle = bm_data->m_prototype->alloc_data_fn(pdata,buffer_size,usage);
if(newbufferhandle==0) return G_BUFFER_OP_INVALID;
GET_AVALIABLE_BUFFER_ID(bm_data,buffer_id->m_buffer_id);
buffer_id->m_bm_data = bm_data;
GBUFFER_DATA * pbuffer = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
pbuffer += buffer_id->m_buffer_id ;
pbuffer->m_buffer_handle = newbufferhandle;
pbuffer->m_size = buffer_size;
pbuffer->m_usage = usage;
pbuffer->m_lock_count = 0;
pbuffer->m_mapped_pointer = 0;
pbuffer->m_refcount = 0;
//set shadow buffer if needed
if(usage == G_MU_STATIC_READ ||
usage == G_MU_STATIC_READ_DYNAMIC_WRITE||
usage == G_MU_STATIC_READ_DYNAMIC_WRITE_COPY)
{
gim_create_common_buffer_from_data(buffer_managers,pdata,buffer_size,&pbuffer->m_shadow_buffer);
}
else
{
pbuffer->m_shadow_buffer.m_bm_data = 0;
pbuffer->m_shadow_buffer.m_buffer_id = G_UINT_INFINITY;
}
return G_BUFFER_OP_SUCCESS;
}
void gim_destroy_buffer_manager(GBUFFER_MANAGER_DATA buffer_managers[], GUINT32 buffer_manager_id)
{
GBUFFER_MANAGER_DATA * bm_data;
gim_get_buffer_manager_data(buffer_managers,buffer_manager_id,&bm_data);
if(bm_data == 0) return;
//Destroy all buffers
GBUFFER_DATA * buffers = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
GUINT32 i, buffer_count = bm_data->m_buffer_array.m_size;
for (i=0;i<buffer_count ;i++ )
{
GBUFFER_DATA * current_buffer = buffers + i;
if(current_buffer->m_buffer_handle!=0) //Is active
{
// free handle
bm_data->m_prototype->free_fn(current_buffer->m_buffer_handle,current_buffer->m_size);
}
}
//destroy buffer array
GIM_DYNARRAY_DESTROY(bm_data->m_buffer_array);
//destroy free positions
GIM_DYNARRAY_DESTROY(bm_data->m_free_positions);
}
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);
}
// capsule - trimesh By francisco leon
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)o1;
dxGeom* gCylinder = o2;
//Get capsule params
dMatrix3 mCapsuleRotation;
dVector3 vCapsulePosition;
dVector3 vCapsuleAxis;
dReal vCapsuleRadius;
dReal fCapsuleSize;
dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
memcpy(vCapsulePosition,pDst,sizeof(dVector3));
//Axis
vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of CCylinder
dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
fCapsuleSize*=0.5f;
//Set Capsule params
GIM_CAPSULE_DATA capsule;
capsule.m_radius = vCapsuleRadius;
VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshe vs capsule
gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned contactmax = (unsigned)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(flags, contact, i, skip);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = TriMesh;
pcontact->g2 = gCylinder;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = -1;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
dVector4 plane;
dGeomPlaneGetParams(o2, plane);
o1 -> recomputeAABB();
o2 -> recomputeAABB();
//Find collision
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result);
if(collision_result.m_size == 0 )
{
GIM_DYNARRAY_DESTROY(collision_result);
return 0;
}
unsigned int contactcount = collision_result.m_size;
unsigned int contactmax = (unsigned int)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
for(unsigned int i = 0; i < contactcount; i++ )
{
pcontact = SAFECONTACT(flags, contacts, i, skip);
pcontact->pos[0] = (*planecontact_results)[0];
pcontact->pos[1] = (*planecontact_results)[1];
pcontact->pos[2] = (*planecontact_results)[2];
pcontact->pos[3] = REAL(1.0);
pcontact->normal[0] = plane[0];
pcontact->normal[1] = plane[1];
pcontact->normal[2] = plane[2];
pcontact->normal[3] = 0;
pcontact->depth = (*planecontact_results)[3];
pcontact->g1 = o1;
pcontact->g2 = o2;
planecontact_results++;
}
GIM_DYNARRAY_DESTROY(collision_result);
return (int)contactcount;
}
int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
{
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (g2->type == dTriMeshClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
g1 -> recomputeAABB();
g2 -> recomputeAABB();
//Collide trimeshes
gim_trimesh_trimesh_collision(&TriMesh1->m_collision_trimesh,&TriMesh2->m_collision_trimesh,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
if (contactcount > maxcontacts)
{
contactcount = maxcontacts;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = g1;
pcontact->g2 = g2;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = ptrimeshcontacts->m_feature2;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
