1
2 /*
3 -----------------------------------------------------------------------------
4 This source file is part of GIMPACT Library.
5
6 For the latest info, see http://gimpact.sourceforge.net/
7
8 Copyright (c) 2006 Francisco Leon. C.C. 80087371.
9 email: projectileman@yahoo.com
10
11 This library is free software; you can redistribute it and/or
12 modify it under the terms of EITHER:
13 (1) The GNU Lesser General Public License as published by the Free
14 Software Foundation; either version 2.1 of the License, or (at
15 your option) any later version. The text of the GNU Lesser
16 General Public License is included with this library in the
17 file GIMPACT-LICENSE-LGPL.TXT.
18 (2) The BSD-style license that is included with this library in
19 the file GIMPACT-LICENSE-BSD.TXT.
20
21 This library is distributed in the hope that it will be useful,
22 but WITHOUT ANY WARRANTY; without even the implied warranty of
23 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
24 GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
25
26 -----------------------------------------------------------------------------
27 */
28
29 #include "GIMPACT/gim_trimesh.h"
30
31 //! Utility function for find the closest point between a segment and a triangle
32 /*!
33
34 \param triangle
35 \param s1
36 \param s2
37 \param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance
38
39 \post The contacts array is not set to 0. It adds aditional contacts
40 */
gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle,vec3f s1,vec3f s2,GDYNAMIC_ARRAY * contacts)41 void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
42 {
43 vec3f segment_points[4] = {{0}};
44 vec3f closest_points[2] = {{0}};
45 GUINT32 intersection_type, out_edge= 10;
46 GREAL dis, dis_temp,perpend;
47 vec4f sdiff;
48
49 dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1);
50 dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2);
51
52 if(dis<=0.0f && dis_temp<=0.0f) return;
53
54 VEC_DIFF(sdiff,s2,s1);
55 perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]);
56
57 if(!IS_ZERO(perpend)) // Not perpendicular
58 {
59 if(dis<dis_temp)
60 {
61 VEC_COPY(closest_points[0],s1);
62 }
63 else
64 {
65 dis = dis_temp;
66 VEC_COPY(closest_points[0],s2);
67 }
68
69 //Testing segment vertices over triangle
70 if(dis>=0.0f && dis_temp>=0.0f)
71 {
72 POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge);
73
74 if(out_edge==0)//Point over face
75 {
76 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
77 return;
78 }
79 }
80 else
81 {
82
83 PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]);
84
85 POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge);
86
87 if(out_edge==0)//Point over face
88 {
89 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
90 return;
91 }
92 }
93
94 }
95 else // Perpendicular Face
96 {
97 //out_edge=10
98 //Clip segment by triangle
99 // Edge1
100 PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type);
101 if(intersection_type==0||intersection_type==1)
102 {
103 out_edge = 0;
104 VEC_COPY(closest_points[0],segment_points[0]);
105 }
106 else
107 {
108 //Edge2
109 PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type);
110 if(intersection_type==0||intersection_type==1)
111 {
112 out_edge = 1;
113 VEC_COPY(closest_points[0],segment_points[3]);
114 }
115 else
116 {
117 //Edge3
118 PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type);
119 if(intersection_type==0||intersection_type==1)
120 {
121 out_edge = 2;
122 }
123 }
124 }
125 //POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2
126 if(out_edge>2) // Over triangle
127 {
128 dis = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[0]);
129 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
130 GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
131 return;
132 }
133 }
134
135 //Find closest edges
136 out_edge = 10;
137 dis = G_REAL_INFINITY;
138 GUINT32 i;
139 for(i=0;i<3;i++)
140 {
141 SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]);
142 VEC_DIFF(sdiff,segment_points[0],segment_points[1]);
143 dis_temp = VEC_DOT(sdiff,sdiff);
144 if(dis_temp< dis)
145 {
146 dis = dis_temp;
147 out_edge = i;
148 VEC_COPY(closest_points[0],segment_points[0]);
149 VEC_COPY(closest_points[1],sdiff);//normal
150 }
151 }
152 if(out_edge>2) return ;// ???? ASSERT this please
153
154 if(IS_ZERO(dis))
155 {
156 //Set face plane
157 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0);
158
159 }
160 else
161 {
162 GIM_SQRT(dis,dis);
163 VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal
164 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0);
165 }
166 }
167
168
169 //! Utility function for find the closest point between a capsule and a triangle
170 /*!
171
172 \param triangle
173 \param capsule
174 \param contacts Contains the closest points on the capsule, and the normal points to triangle
175
176 \post The contacts array is not set to 0. It adds aditional contacts
177 */
gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle,GIM_CAPSULE_DATA * capsule,GDYNAMIC_ARRAY * contacts)178 int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
179 {
180 GUINT32 old_contact_size = contacts->m_size;
181 gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
182
183 if (contacts->m_size == old_contact_size)
184 {
185 return 0;
186 }
187
188 GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
189 pcontact+= old_contact_size;
190
191 if(pcontact->m_depth > capsule->m_radius)
192 {
193 contacts->m_size = old_contact_size;
194 return 0;
195 }
196
197 vec3f vec;
198 while(old_contact_size<contacts->m_size)
199 {
200 //Scale the normal for pointing to triangle
201 VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
202 //Fix the contact point
203 VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
204 VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
205 //Fix the depth
206 pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
207
208 pcontact++;
209 old_contact_size++;
210 }
211
212 return 1;
213 }
214
215
216 //! Trimesh Capsule collision
217 /*!
218 Find the closest primitive collided by the ray
219 \param trimesh
220 \param capsule
221 \param contact
222 \param contacts A GIM_CONTACT array. Must be initialized
223 */
gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh,GIM_CAPSULE_DATA * capsule,GDYNAMIC_ARRAY * contacts)224 void gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
225 {
226 contacts->m_size = 0;
227
228 aabb3f test_aabb;
229 CALC_CAPSULE_AABB((*capsule),test_aabb);
230
231 GDYNAMIC_ARRAY collision_result;
232 GIM_CREATE_BOXQUERY_LIST(collision_result);
233
234 gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
235
236 if(collision_result.m_size==0)
237 {
238 GIM_DYNARRAY_DESTROY(collision_result);
239 }
240
241 //collide triangles
242 //Locks trimesh
243 gim_trimesh_locks_work_data(trimesh);
244 //dummy contacts
245 GDYNAMIC_ARRAY dummycontacts;
246 GIM_CREATE_CONTACT_LIST(dummycontacts);
247
248 int cresult;
249 unsigned int i;
250 GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
251 GIM_TRIANGLE_DATA tri_data;
252 GUINT32 old_contact_size;
253 GIM_CONTACT * pcontact;
254
255 for(i=0;i<collision_result.m_size;i++)
256 {
257 old_contact_size = dummycontacts.m_size;
258 gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
259 cresult = gim_triangle_capsule_collision(&tri_data, capsule, &dummycontacts);
260 if(cresult!=0)
261 {
262 pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,dummycontacts);
263 pcontact+= old_contact_size;
264 while(old_contact_size<dummycontacts.m_size)
265 {
266 pcontact->m_handle1 = trimesh;
267 pcontact->m_handle2 = capsule;
268 pcontact->m_feature1 = boxesresult[i];
269 pcontact->m_feature2 = 0;
270 pcontact++;
271 old_contact_size++;
272 }
273 }
274 }
275 ///unlocks
276 gim_trimesh_unlocks_work_data(trimesh);
277 ///Destroy box result
278 GIM_DYNARRAY_DESTROY(collision_result);
279
280 //merge contacts
281 gim_merge_contacts(&dummycontacts,contacts);
282
283 //Destroy dummy
284 GIM_DYNARRAY_DESTROY(dummycontacts);
285 }
286