diff options
Diffstat (limited to 'libs/ode-0.16.1/GIMPACT/src/gim_trimesh_capsule_collision.cpp')
| -rw-r--r-- | libs/ode-0.16.1/GIMPACT/src/gim_trimesh_capsule_collision.cpp | 285 |
1 files changed, 285 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/GIMPACT/src/gim_trimesh_capsule_collision.cpp b/libs/ode-0.16.1/GIMPACT/src/gim_trimesh_capsule_collision.cpp new file mode 100644 index 0000000..bc48aa5 --- /dev/null +++ b/libs/ode-0.16.1/GIMPACT/src/gim_trimesh_capsule_collision.cpp @@ -0,0 +1,285 @@ + +/* +----------------------------------------------------------------------------- +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" + +//! Utility function for find the closest point between a segment and a triangle +/*! + +\param triangle +\param s1 +\param s2 +\param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance + +\post The contacts array is not set to 0. It adds aditional contacts +*/ +void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts) +{ + vec3f segment_points[4] = {{0}}; + vec3f closest_points[2] = {{0}}; + GUINT32 intersection_type, out_edge= 10; + GREAL dis, dis_temp,perpend; + vec4f sdiff; + + dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1); + dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2); + + if(dis<=0.0f && dis_temp<=0.0f) return; + + VEC_DIFF(sdiff,s2,s1); + perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]); + + if(!IS_ZERO(perpend)) // Not perpendicular + { + if(dis<dis_temp) + { + VEC_COPY(closest_points[0],s1); + } + else + { + dis = dis_temp; + VEC_COPY(closest_points[0],s2); + } + + //Testing segment vertices over triangle + if(dis>=0.0f && dis_temp>=0.0f) + { + POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge); + + if(out_edge==0)//Point over face + { + GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0); + return; + } + } + else + { + + PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]); + + POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge); + + if(out_edge==0)//Point over face + { + GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0); + return; + } + } + + } + else // Perpendicular Face + { + //out_edge=10 + //Clip segment by triangle + // Edge1 + PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type); + if(intersection_type==0||intersection_type==1) + { + out_edge = 0; + VEC_COPY(closest_points[0],segment_points[0]); + } + else + { + //Edge2 + PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type); + if(intersection_type==0||intersection_type==1) + { + out_edge = 1; + VEC_COPY(closest_points[0],segment_points[3]); + } + else + { + //Edge3 + PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type); + if(intersection_type==0||intersection_type==1) + { + out_edge = 2; + } + } + } + //POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2 + if(out_edge>2) // Over triangle + { + dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],closest_points[0]); + GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0); + GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0); + return; + } + } + + //Find closest edges + out_edge = 10; + dis = G_REAL_INFINITY; + GUINT32 i; + for(i=0;i<3;i++) + { + SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]); + VEC_DIFF(sdiff,segment_points[0],segment_points[1]); + dis_temp = VEC_DOT(sdiff,sdiff); + if(dis_temp< dis) + { + dis = dis_temp; + out_edge = i; + VEC_COPY(closest_points[0],segment_points[0]); + VEC_COPY(closest_points[1],sdiff);//normal + } + } + if(out_edge>2) return ;// ???? ASSERT this please + + if(IS_ZERO(dis)) + { + //Set face plane + GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0); + + } + else + { + GIM_SQRT(dis,dis); + VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal + GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0); + } +} + + +//! Utility function for find the closest point between a capsule and a triangle +/*! + +\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) +{ + GUINT32 old_contact_size = contacts->m_size; + gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts); + + if (contacts->m_size == old_contact_size) + { + return 0; + } + + 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; +} + + +//! Trimesh Capsule collision +/*! +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; + GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); + GIM_TRIANGLE_DATA tri_data; + GUINT32 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); +} |