1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
|
/*
-----------------------------------------------------------------------------
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];
vec3f closest_points[2];
GUINT 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 = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[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;
GUINT 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)
{
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;
}
//! 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;
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);
}
|
