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; }