aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp')
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp279
1 files changed, 279 insertions, 0 deletions
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp
new file mode 100644
index 0000000..d1f7ca3
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp
@@ -0,0 +1,279 @@
1
2/*
3-----------------------------------------------------------------------------
4This source file is part of GIMPACT Library.
5
6For the latest info, see http://gimpact.sourceforge.net/
7
8Copyright (c) 2006 Francisco Leon. C.C. 80087371.
9email: 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*/
41void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
42{
43 vec3f segment_points[4];
44 vec3f closest_points[2];
45 GUINT 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 GUINT 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*/
178int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
179{
180 GUINT old_contact_size = contacts->m_size;
181 gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
182 GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
183 pcontact+= old_contact_size;
184
185 if(pcontact->m_depth > capsule->m_radius)
186 {
187 contacts->m_size = old_contact_size;
188 return 0;
189 }
190
191 vec3f vec;
192 while(old_contact_size<contacts->m_size)
193 {
194 //Scale the normal for pointing to triangle
195 VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
196 //Fix the contact point
197 VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
198 VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
199 //Fix the depth
200 pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
201
202 pcontact++;
203 old_contact_size++;
204 }
205
206 return 1;
207}
208
209
210//! Trimesh Capsule collision
211/*!
212Find the closest primitive collided by the ray
213\param trimesh
214\param capsule
215\param contact
216\param contacts A GIM_CONTACT array. Must be initialized
217*/
218void gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
219{
220 contacts->m_size = 0;
221
222 aabb3f test_aabb;
223 CALC_CAPSULE_AABB((*capsule),test_aabb);
224
225 GDYNAMIC_ARRAY collision_result;
226 GIM_CREATE_BOXQUERY_LIST(collision_result);
227
228 gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
229
230 if(collision_result.m_size==0)
231 {
232 GIM_DYNARRAY_DESTROY(collision_result);
233 }
234
235 //collide triangles
236 //Locks trimesh
237 gim_trimesh_locks_work_data(trimesh);
238 //dummy contacts
239 GDYNAMIC_ARRAY dummycontacts;
240 GIM_CREATE_CONTACT_LIST(dummycontacts);
241
242 int cresult;
243 unsigned int i;
244 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
245 GIM_TRIANGLE_DATA tri_data;
246 GUINT old_contact_size;
247 GIM_CONTACT * pcontact;
248
249 for(i=0;i<collision_result.m_size;i++)
250 {
251 old_contact_size = dummycontacts.m_size;
252 gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
253 cresult = gim_triangle_capsule_collision(&tri_data, capsule, &dummycontacts);
254 if(cresult!=0)
255 {
256 pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,dummycontacts);
257 pcontact+= old_contact_size;
258 while(old_contact_size<dummycontacts.m_size)
259 {
260 pcontact->m_handle1 = trimesh;
261 pcontact->m_handle2 = capsule;
262 pcontact->m_feature1 = boxesresult[i];
263 pcontact->m_feature2 = 0;
264 pcontact++;
265 old_contact_size++;
266 }
267 }
268 }
269 ///unlocks
270 gim_trimesh_unlocks_work_data(trimesh);
271 ///Destroy box result
272 GIM_DYNARRAY_DESTROY(collision_result);
273
274 //merge contacts
275 gim_merge_contacts(&dummycontacts,contacts);
276
277 //Destroy dummy
278 GIM_DYNARRAY_DESTROY(dummycontacts);
279}
generated by cgit v1.1 at 2024-10-15 17:26:41 +0000