/*! \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); }