aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/GIMPACT/src
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:48 +0000
committerdan miller2007-10-19 05:20:48 +0000
commitd48ea5bb797037069d641da41da0f195f0124491 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/GIMPACT/src
parentdont ask (diff)
downloadopensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.zip
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.gz
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.bz2
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.xz
one more for the gipper
Diffstat (limited to 'libraries/ode-0.9/GIMPACT/src')
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp514
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_contact.cpp132
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_math.cpp60
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_memory.cpp848
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_tri_tri_overlap.cpp251
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh.cpp364
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_capsule_collision.cpp279
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_ray_collision.cpp140
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp196
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_trimesh_collision.cpp348
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gimpact.cpp39
11 files changed, 3171 insertions, 0 deletions
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp b/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp
new file mode 100644
index 0000000..91e1d37
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp
@@ -0,0 +1,514 @@
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
30#include "GIMPACT/gim_boxpruning.h"
31
32
33
34//! Allocate memory for all aabb set.
35void gim_aabbset_alloc(GIM_AABB_SET * aabbset, GUINT count)
36{
37 aabbset->m_count = count;
38 aabbset->m_boxes = (aabb3f *)gim_alloc(sizeof(aabb3f)*count);
39
40 if(count<GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
41 {
42 aabbset->m_maxcoords = 0;
43 aabbset->m_sorted_mincoords = 0;
44 }
45 else
46 {
47 aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
48 aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
49 }
50 aabbset->m_shared = 0;
51 INVALIDATE_AABB(aabbset->m_global_bound);
52}
53
54//! Destroys the aabb set.
55void gim_aabbset_destroy(GIM_AABB_SET * aabbset)
56{
57 aabbset->m_count = 0;
58 if(aabbset->m_shared==0)
59 {
60 gim_free(aabbset->m_boxes,0);
61 gim_free(aabbset->m_maxcoords,0);
62 gim_free(aabbset->m_sorted_mincoords,0);
63 }
64 aabbset->m_boxes = 0;
65 aabbset->m_sorted_mincoords = 0;
66 aabbset->m_maxcoords = 0;
67}
68
69void gim_aabbset_calc_global_bound(GIM_AABB_SET * aabbset)
70{
71 aabb3f * paabb = aabbset->m_boxes;
72 aabb3f * globalbox = &aabbset->m_global_bound;
73 AABB_COPY((*globalbox),(*paabb));
74
75 GUINT count = aabbset->m_count-1;
76 paabb++;
77 while(count)
78 {
79 MERGEBOXES(*globalbox,*paabb)
80 paabb++;
81 count--;
82 }
83}
84
85
86//! Sorts the boxes for box prunning.
87/*!
881) find the integer representation of the aabb coords
892) Sorts the min coords
903) Calcs the global bound
91\pre aabbset must be allocated. And the boxes must be already set.
92\param aabbset
93\param calc_global_bound If 1 , calcs the global bound
94\post If aabbset->m_sorted_mincoords == 0, then it allocs the sorted coordinates
95*/
96void gim_aabbset_sort(GIM_AABB_SET * aabbset, char calc_global_bound)
97{
98 if(aabbset->m_sorted_mincoords == 0)
99 {//allocate
100 aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
101 aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
102 }
103
104 GUINT i, count = aabbset->m_count;
105 aabb3f * paabb = aabbset->m_boxes;
106 GUINT * maxcoords = aabbset->m_maxcoords;
107 GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
108
109 if(count<860)//Calibrated on a Pentium IV
110 {
111 //Sort by quick sort
112 //Calculate keys
113 for(i=0;i<count;i++)
114 {
115 GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
116 GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,sorted_tokens[i].m_key);
117 sorted_tokens[i].m_value = i;
118 }
119 GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , sorted_tokens, count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
120 }
121 else
122 {
123 //Sort by radix sort
124 GIM_RSORT_TOKEN * unsorted = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN )*count);
125 //Calculate keys
126 for(i=0;i<count;i++)
127 {
128 GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
129 GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,unsorted[i].m_key);
130 unsorted[i].m_value = i;
131 }
132 GIM_RADIX_SORT_RTOKENS(unsorted,sorted_tokens,count);
133 gim_free(unsorted,0);
134 }
135
136 if(calc_global_bound) gim_aabbset_calc_global_bound(aabbset);
137}
138
139//utility macros
140
141/*#define PUSH_PAIR(i,j,pairset)\
142{\
143 GIM_PAIR _pair={i,j};\
144 GIM_DYNARRAY_PUSH_ITEM(GIM_PAIR,pairset,_pair);\
145}*/
146
147#define PUSH_PAIR(i,j,pairset)\
148{\
149 GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
150 GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
151 _pair->m_index1 = i;\
152 _pair->m_index2 = j;\
153}
154
155#define PUSH_PAIR_INV(i,j,pairset)\
156{\
157 GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
158 GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
159 _pair->m_index1 = j;\
160 _pair->m_index2 = i;\
161}
162
163#define FIND_OVERLAPPING_FOWARD(\
164 curr_index,\
165 test_count,\
166 test_aabb,\
167 max_coord_uint,\
168 sorted_tokens,\
169 aabbarray,\
170 pairset,\
171 push_pair_macro)\
172{\
173 GUINT _i = test_count;\
174 char _intersected;\
175 GIM_RSORT_TOKEN * _psorted_tokens = sorted_tokens;\
176 while(max_coord_uint >= _psorted_tokens->m_key && _i>0)\
177 {\
178 AABBCOLLISION(_intersected,test_aabb,aabbarray[_psorted_tokens->m_value]);\
179 if(_intersected)\
180 {\
181 push_pair_macro(curr_index, _psorted_tokens->m_value,pairset);\
182 }\
183 _psorted_tokens++;\
184 _i--;\
185 }\
186}
187
188//! log(N) Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
189/*!
190\pre aabbset must be allocated and sorted, the boxes must be already set.
191\param aabbset Must be sorted. Global bound isn't required
192\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
193*/
194void gim_aabbset_self_intersections_sorted(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
195{
196 collision_pairs->m_size = 0;
197 GUINT count = aabbset->m_count;
198 aabb3f * paabb = aabbset->m_boxes;
199 GUINT * maxcoords = aabbset->m_maxcoords;
200 GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
201 aabb3f test_aabb;
202 while(count>1)
203 {
204 ///current cache variables
205 GUINT curr_index = sorted_tokens->m_value;
206 GUINT max_coord_uint = maxcoords[curr_index];
207 AABB_COPY(test_aabb,paabb[curr_index]);
208
209 ///next pairs
210 sorted_tokens++;
211 count--;
212 FIND_OVERLAPPING_FOWARD( curr_index, count, test_aabb, max_coord_uint, sorted_tokens , paabb, (*collision_pairs),PUSH_PAIR);
213 }
214}
215
216//! NxN Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
217/*!
218\pre aabbset must be allocated, the boxes must be already set.
219\param aabbset Global bound isn't required. Doen't need to be sorted.
220\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
221*/
222void gim_aabbset_self_intersections_brute_force(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
223{
224 collision_pairs->m_size = 0;
225 GUINT i,j;
226 GUINT count = aabbset->m_count;
227 aabb3f * paabb = aabbset->m_boxes;
228 char intersected;
229 for (i=0;i< count-1 ;i++ )
230 {
231 for (j=i+1;j<count ;j++ )
232 {
233 AABBCOLLISION(intersected,paabb[i],paabb[j]);
234 if(intersected)
235 {
236 PUSH_PAIR(i,j,(*collision_pairs));
237 }
238 }
239 }
240}
241
242//! log(N) Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
243/*!
244\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
245\param aabbset1 Must be sorted, Global bound is required.
246\param aabbset2 Must be sorted, Global bound is required.
247\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
248*/
249void gim_aabbset_bipartite_intersections_sorted(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
250{
251 char intersected;
252 collision_pairs->m_size = 0;
253
254 AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
255 if(intersected == 0) return;
256
257 GUINT count1 = aabbset1->m_count;
258 aabb3f * paabb1 = aabbset1->m_boxes;
259 GUINT * maxcoords1 = aabbset1->m_maxcoords;
260 GIM_RSORT_TOKEN * sorted_tokens1 = aabbset1->m_sorted_mincoords;
261
262 GUINT count2 = aabbset2->m_count;
263 aabb3f * paabb2 = aabbset2->m_boxes;
264 GUINT * maxcoords2 = aabbset2->m_maxcoords;
265 GIM_RSORT_TOKEN * sorted_tokens2 = aabbset2->m_sorted_mincoords;
266
267 GUINT curr_index;
268
269 GUINT max_coord_uint;
270 aabb3f test_aabb;
271
272 //Classify boxes
273 //Find Set intersection
274 aabb3f int_abbb;
275 BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
276
277 //Clasify set 1
278 GIM_RSORT_TOKEN * classified_tokens1 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count1);
279 GUINT i,classified_count1 = 0,classified_count2 = 0;
280
281
282 for (i=0;i<count1;i++ )
283 {
284 curr_index = sorted_tokens1[i].m_value;
285 AABBCOLLISION(intersected,paabb1[curr_index],int_abbb);
286 if(intersected)
287 {
288 classified_tokens1[classified_count1] = sorted_tokens1[i];
289 classified_count1++;
290 }
291 }
292
293 if(classified_count1==0)
294 {
295 gim_free(classified_tokens1 ,0);
296 return; // no pairs
297 }
298
299 //Clasify set 2
300 GIM_RSORT_TOKEN * classified_tokens2 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count2);
301
302 for (i=0;i<count2;i++ )
303 {
304 curr_index = sorted_tokens2[i].m_value;
305 AABBCOLLISION(intersected,paabb2[curr_index],int_abbb);
306 if(intersected)
307 {
308 classified_tokens2[classified_count2] = sorted_tokens2[i];
309 classified_count2++;
310 }
311 }
312
313 if(classified_count2==0)
314 {
315 gim_free(classified_tokens1 ,0);
316 gim_free(classified_tokens2 ,0);
317 return; // no pairs
318 }
319
320 sorted_tokens1 = classified_tokens1;
321 sorted_tokens2 = classified_tokens2;
322
323 while(classified_count1>0&&classified_count2>0)
324 {
325 if(sorted_tokens1->m_key <= sorted_tokens2->m_key)
326 {
327 ///current cache variables
328 curr_index = sorted_tokens1->m_value;
329 max_coord_uint = maxcoords1[curr_index];
330 AABB_COPY(test_aabb,paabb1[curr_index]);
331 ///next pairs
332 sorted_tokens1++;
333 classified_count1--;
334 FIND_OVERLAPPING_FOWARD( curr_index, classified_count2, test_aabb, max_coord_uint, sorted_tokens2 , paabb2, (*collision_pairs), PUSH_PAIR);
335 }
336 else ///Switch test
337 {
338 ///current cache variables
339 curr_index = sorted_tokens2->m_value;
340 max_coord_uint = maxcoords2[curr_index];
341 AABB_COPY(test_aabb,paabb2[curr_index]);
342 ///next pairs
343 sorted_tokens2++;
344 classified_count2--;
345 FIND_OVERLAPPING_FOWARD( curr_index, classified_count1, test_aabb, max_coord_uint, sorted_tokens1 , paabb1, (*collision_pairs), PUSH_PAIR_INV );
346 }
347 }
348 gim_free(classified_tokens1 ,0);
349 gim_free(classified_tokens2 ,0);
350}
351
352//! NxM Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
353/*!
354\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
355\param aabbset1 Must be sorted, Global bound is required.
356\param aabbset2 Must be sorted, Global bound is required.
357\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
358*/
359void gim_aabbset_bipartite_intersections_brute_force(GIM_AABB_SET * aabbset1,GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
360{
361 char intersected;
362 collision_pairs->m_size = 0;
363 AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
364 if(intersected == 0) return;
365
366 aabb3f int_abbb;
367 //Find Set intersection
368 BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
369 //Clasify set 1
370 GUINT i,j;
371 GUINT classified_count = 0;
372
373 GUINT count = aabbset1->m_count;
374 aabb3f * paabb1 = aabbset1->m_boxes;
375 aabb3f * paabb2 = aabbset2->m_boxes;
376
377 GUINT * classified = (GUINT *) gim_alloc(sizeof(GUINT)*count);
378
379 for (i=0;i<count;i++ )
380 {
381 AABBCOLLISION(intersected,paabb1[i],int_abbb);
382 if(intersected)
383 {
384 classified[classified_count] = i;
385 classified_count++;
386 }
387 }
388
389 if(classified_count==0) return; // no pairs
390
391 //intesect set2
392 count = aabbset2->m_count;
393 for (i=0;i<count;i++)
394 {
395 AABBCOLLISION(intersected,paabb2[i],int_abbb);
396 if(intersected)
397 {
398 for (j=0;j<classified_count;j++)
399 {
400 AABBCOLLISION(intersected,paabb2[i],paabb1[classified[j]]);
401 if(intersected)
402 {
403 PUSH_PAIR(classified[j],i,(*collision_pairs));
404 }
405 }
406 }
407 }
408 gim_free(classified,0);
409}
410
411
412//! Initalizes the set. Sort Boxes if needed.
413/*!
414\pre aabbset must be allocated. And the boxes must be already set.
415\post If the set has less of GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES boxes, only calcs the global box,
416 else it Sorts the entire set( Only applicable for large sets)
417*/
418void gim_aabbset_update(GIM_AABB_SET * aabbset)
419{
420 if(aabbset->m_count < GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
421 {//Brute force approach
422 gim_aabbset_calc_global_bound(aabbset);
423 }
424 else
425 {//Sorted force approach
426 gim_aabbset_sort(aabbset,1);
427 }
428}
429
430//! Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
431/*!
432This function sorts the set and then it calls to gim_aabbset_self_intersections_brute_force or gim_aabbset_self_intersections_sorted.
433
434\param aabbset Set of boxes. Sorting isn't required.
435\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
436\pre aabbset must be allocated and initialized.
437\post If aabbset->m_count >= GIM_MIN_SORTED_PRUNING_BOXES, then it calls to gim_aabbset_sort and then to gim_aabbset_self_intersections_sorted.
438*/
439void gim_aabbset_self_intersections(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
440{
441 if(aabbset->m_count < GIM_MIN_SORTED_PRUNING_BOXES)
442 {//Brute force approach
443 gim_aabbset_self_intersections_brute_force(aabbset,collision_pairs);
444 }
445 else
446 {//Sorted force approach
447 gim_aabbset_sort(aabbset,0);
448 gim_aabbset_self_intersections_sorted(aabbset,collision_pairs);
449 }
450}
451
452//! Collides two sets. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
453/*!
454\pre aabbset1 and aabbset2 must be allocated and updated. See .
455\param aabbset1 Must be sorted, Global bound is required.
456\param aabbset2 Must be sorted, Global bound is required.
457\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
458*/
459void gim_aabbset_bipartite_intersections(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
460{
461 if(aabbset1->m_sorted_mincoords == 0||aabbset2->m_sorted_mincoords == 0)
462 {//Brute force approach
463 gim_aabbset_bipartite_intersections_brute_force(aabbset1,aabbset2,collision_pairs);
464 }
465 else
466 {//Sorted force approach
467 gim_aabbset_bipartite_intersections_sorted(aabbset1,aabbset2,collision_pairs);
468 }
469}
470
471void gim_aabbset_box_collision(aabb3f *test_aabb, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
472{
473 collided->m_size = 0;
474 char intersected;
475 AABBCOLLISION(intersected,aabbset->m_global_bound,(*test_aabb));
476 if(intersected == 0) return;
477
478 GUINT i;
479 GUINT count = aabbset->m_count;
480 aabb3f * paabb = aabbset->m_boxes;
481 aabb3f _testaabb;
482 AABB_COPY(_testaabb,*test_aabb);
483
484 for (i=0;i< count;i++ )
485 {
486 AABBCOLLISION(intersected,paabb[i],_testaabb);
487 if(intersected)
488 {
489 GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
490 }
491 }
492}
493
494void gim_aabbset_ray_collision(vec3f vorigin,vec3f vdir, GREAL tmax, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
495{
496 collided->m_size = 0;
497 char intersected;
498 GREAL tparam = 0;
499 BOX_INTERSECTS_RAY(aabbset->m_global_bound, vorigin, vdir, tparam, tmax,intersected);
500 if(intersected==0) return;
501
502 GUINT i;
503 GUINT count = aabbset->m_count;
504 aabb3f * paabb = aabbset->m_boxes;
505
506 for (i=0;i< count;i++ )
507 {
508 BOX_INTERSECTS_RAY(paabb[i], vorigin, vdir, tparam, tmax,intersected);
509 if(intersected)
510 {
511 GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
512 }
513 }
514}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp
new file mode 100644
index 0000000..762af06
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_contact.cpp
@@ -0,0 +1,132 @@
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_contact.h"
30
31void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
32 GDYNAMIC_ARRAY * dest_contacts)
33{
34 dest_contacts->m_size = 0;
35
36 GUINT source_count = source_contacts->m_size;
37 GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
38 //create keys
39 GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);
40
41 GUINT i;
42 for(i=0;i<source_count;i++)
43 {
44 keycontacts[i].m_value = i;
45 GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
46 }
47
48 //sort keys
49 GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
50
51 // Merge contacts
52 GIM_CONTACT * pcontact = 0;
53 GIM_CONTACT * scontact = 0;
54 GUINT key,last_key=0;
55
56 for(i=0;i<source_contacts->m_size;i++)
57 {
58 key = keycontacts[i].m_key;
59 scontact = &psource_contacts[keycontacts[i].m_value];
60
61 if(i>0 && last_key == key)
62 {
63 //merge contact
64 if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
65 {
66 GIM_COPY_CONTACTS(pcontact, scontact);
67 }
68 }
69 else
70 {//add new contact
71 GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
72 pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
73 GIM_COPY_CONTACTS(pcontact, scontact);
74 }
75 last_key = key;
76 }
77 gim_free(keycontacts,0);
78}
79
80void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
81 GDYNAMIC_ARRAY * dest_contacts)
82{
83 dest_contacts->m_size = 0;
84 //Traverse the source contacts
85 GUINT source_count = source_contacts->m_size;
86 if(source_count==0) return;
87
88 GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
89
90 //add the unique contact
91 GIM_CONTACT * pcontact = 0;
92 GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
93 pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
94 //set the first contact
95 GIM_COPY_CONTACTS(pcontact, psource_contacts);
96
97 if(source_count==1) return;
98 //scale the first contact
99 VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
100
101 psource_contacts++;
102
103 //Average the contacts
104 GUINT i;
105 for(i=1;i<source_count;i++)
106 {
107 VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
108 VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
109 psource_contacts++;
110 }
111
112 GREAL divide_average = 1.0f/((GREAL)source_count);
113
114 VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
115
116 pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
117 GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
118
119 VEC_NORMALIZE(pcontact->m_normal);
120
121 /*GREAL normal_len;
122 VEC_INV_LENGTH(pcontact->m_normal,normal_len);
123 VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
124
125 //Deep = LEN(normal)/SQRT(source_count)
126 GIM_SQRT(divide_average,divide_average);
127 pcontact->m_depth = divide_average/normal_len;
128 */
129}
130
131
132
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_math.cpp b/libraries/ode-0.9/GIMPACT/src/gim_math.cpp
new file mode 100644
index 0000000..18efb2c
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_math.cpp
@@ -0,0 +1,60 @@
1/*
2-----------------------------------------------------------------------------
3This source file is part of GIMPACT Library.
4
5For the latest info, see http://gimpact.sourceforge.net/
6
7Copyright (c) 2006 Francisco Leon. C.C. 80087371.
8email: projectileman@yahoo.com
9
10 This library is free software; you can redistribute it and/or
11 modify it under the terms of EITHER:
12 (1) The GNU Lesser General Public License as published by the Free
13 Software Foundation; either version 2.1 of the License, or (at
14 your option) any later version. The text of the GNU Lesser
15 General Public License is included with this library in the
16 file GIMPACT-LICENSE-LGPL.TXT.
17 (2) The BSD-style license that is included with this library in
18 the file GIMPACT-LICENSE-BSD.TXT.
19
20 This library is distributed in the hope that it will be useful,
21 but WITHOUT ANY WARRANTY; without even the implied warranty of
22 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
23 GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
24
25-----------------------------------------------------------------------------
26*/
27
28
29#include "GIMPACT/gim_math.h"
30#include "stdlib.h"
31#include "time.h"
32
33
34GREAL gim_inv_sqrt(GREAL f)
35{
36 GREAL r;
37 GIM_INV_SQRT(f,r);
38 return r;
39}
40
41GREAL gim_sqrt(GREAL f)
42{
43 GREAL r;
44 GIM_SQRT(f,r);
45 return r;
46}
47
48//!Initializes mathematical functions
49void gim_init_math()
50{
51 srand( static_cast< unsigned int >( time( 0 ) ) );
52}
53
54//! Generates an unit random
55GREAL gim_unit_random()
56{
57 GREAL rn = static_cast< GREAL >( rand() );
58 rn/=(GREAL)RAND_MAX;
59 return rn;
60}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_memory.cpp b/libraries/ode-0.9/GIMPACT/src/gim_memory.cpp
new file mode 100644
index 0000000..247565a
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_memory.cpp
@@ -0,0 +1,848 @@
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
30#include "GIMPACT/gim_memory.h"
31#include "stdlib.h"
32#include "malloc.h"
33//#include "mm_malloc.h"
34
35static gim_alloc_function *g_allocfn = 0;
36static gim_alloca_function *g_allocafn = 0;
37static gim_realloc_function *g_reallocfn = 0;
38static gim_free_function *g_freefn = 0;
39
40// buffer managers
41#define MAX_BUFFER_MANAGERS 16
42static GBUFFER_MANAGER_DATA g_buffer_managers[MAX_BUFFER_MANAGERS];
43static GUINT g_buffer_managers_count = 0;
44
45#define VALIDATE_BUFFER_MANAGER(buffer_manager_id)\
46 if(buffer_manager_id>=MAX_BUFFER_MANAGERS) return G_BUFFER_OP_INVALID;\
47 GBUFFER_MANAGER_DATA * bm_data;\
48 gim_get_buffer_manager_data(buffer_manager_id,&bm_data);\
49 if(bm_data == 0) return G_BUFFER_OP_INVALID;\
50
51#define VALIDATE_BUFFER_ID_PT(buffer_id)\
52 VALIDATE_BUFFER_MANAGER(buffer_id->m_buffer_manager_id)\
53 if(buffer_id->m_buffer_id>=bm_data->m_buffer_array.m_size) return G_BUFFER_OP_INVALID;\
54 GBUFFER_DATA * pbuffer = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);\
55 pbuffer += buffer_id->m_buffer_id;\
56 if(pbuffer->m_buffer_handle==0) return G_BUFFER_OP_INVALID;\
57
58
59void GIM_BUFFER_ARRAY_DESTROY(GBUFFER_ARRAY & array_data)
60{
61 gim_buffer_array_unlock(&array_data);
62 gim_buffer_free(&(array_data).m_buffer_id);
63}
64
65void GIM_DYNARRAY_DESTROY(GDYNAMIC_ARRAY & array_data)
66{
67 if(array_data.m_pdata != 0)
68 {
69 gim_free(array_data.m_pdata,0);
70 array_data.m_reserve_size = 0;
71 array_data.m_size = 0;
72 array_data.m_pdata = 0;
73 }
74}
75
76void gim_set_alloc_handler (gim_alloc_function *fn)
77{
78 g_allocfn = fn;
79}
80
81void gim_set_alloca_handler (gim_alloca_function *fn)
82{
83 g_allocafn = fn;
84}
85
86void gim_set_realloc_handler (gim_realloc_function *fn)
87{
88 g_reallocfn = fn;
89}
90
91void gim_set_free_handler (gim_free_function *fn)
92{
93 g_freefn = fn;
94}
95
96gim_alloc_function *gim_get_alloc_handler()
97{
98 return g_allocfn;
99}
100
101gim_alloca_function *gim_get_alloca_handler()
102{
103 return g_allocafn;
104}
105
106
107gim_realloc_function *gim_get_realloc_handler ()
108{
109 return g_reallocfn;
110}
111
112
113gim_free_function *gim_get_free_handler ()
114{
115 return g_freefn;
116}
117
118
119void * gim_alloc(size_t size)
120{
121 void * ptr = 0;
122 ptr = malloc(size);
123 /*if (g_allocfn) ptr = g_allocfn(size); else ptr = malloc(size);//_mm_malloc(size,0);*/
124 if(ptr==0)
125 {
126 float * fp = 0;
127 *fp = 0.0f;
128 }
129 return ptr;
130}
131
132void * gim_alloca(size_t size)
133{
134 if (g_allocafn) return g_allocafn(size); else return alloca(size);
135}
136
137
138void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
139{
140 /*if (g_reallocfn) return g_reallocfn(ptr,oldsize,newsize);
141 else return realloc(ptr,newsize);*/
142 //return realloc(ptr,newsize);
143 void * newptr = gim_alloc(newsize);
144 size_t copysize = newsize> oldsize? oldsize: newsize;
145 memcpy(newptr,ptr,copysize);
146 gim_free(ptr,oldsize);
147 return newptr;
148}
149
150void gim_free(void *ptr, size_t size)
151{
152 if (!ptr) return;
153 if (g_freefn)
154 {
155 g_freefn(ptr,size);
156 }
157 else
158 {
159 free(ptr);//_mm_free(ptr);
160 }
161}
162
163///******************************* BUFFER MANAGERS ******************************///
164
165//!** Basic buffer prototyoe functions
166
167GUINT _system_buffer_alloc_function(GUINT size,int usage)
168{
169 void * newdata = gim_alloc(size);
170 memset(newdata,0,size);
171 return (GUINT)(newdata);
172}
173
174GUINT _system_buffer_alloc_data_function(const void * pdata,GUINT size,int usage)
175{
176 void * newdata = gim_alloc(size);
177 memcpy(newdata,pdata,size);
178 return (GUINT)(newdata);
179}
180
181GUINT _system_buffer_realloc_function(GUINT buffer_handle,GUINT oldsize,int old_usage,GUINT newsize,int new_usage)
182{
183 void * newdata = gim_realloc((void *)buffer_handle,oldsize,newsize);
184 return (GUINT)(newdata);
185}
186
187void _system_buffer_free_function(GUINT buffer_handle,GUINT size)
188{
189 gim_free((void*)buffer_handle,size);
190}
191
192char * _system_lock_buffer_function(GUINT buffer_handle,int access)
193{
194 return (char * )(buffer_handle);
195}
196
197
198void _system_unlock_buffer_function(GUINT buffer_handle)
199{
200}
201
202void _system_download_from_buffer_function(
203 GUINT source_buffer_handle,
204 GUINT source_pos,
205 void * destdata,
206 GUINT copysize)
207{
208 char * pdata;
209 pdata = (char *)source_buffer_handle;
210 memcpy(destdata,pdata+source_pos,copysize);
211}
212
213void _system_upload_to_buffer_function(
214 GUINT dest_buffer_handle,
215 GUINT dest_pos,
216 void * sourcedata,
217 GUINT copysize)
218{
219 char * pdata;
220 pdata = (char * )dest_buffer_handle;
221 memcpy(pdata+dest_pos,sourcedata,copysize);
222}
223
224void _system_copy_buffers_function(
225 GUINT source_buffer_handle,
226 GUINT source_pos,
227 GUINT dest_buffer_handle,
228 GUINT dest_pos,
229 GUINT copysize)
230{
231 char * pdata1,*pdata2;
232 pdata1 = (char *)source_buffer_handle;
233 pdata2 = (char *)dest_buffer_handle;
234 memcpy(pdata2+dest_pos,pdata1+source_pos,copysize);
235}
236
237GUINT _shared_buffer_alloc_function(GUINT size,int usage)
238{
239 return 0;
240}
241
242GUINT _shared_buffer_alloc_data_function(const void * pdata,GUINT size,int usage)
243{
244 return (GUINT)pdata;
245}
246
247GUINT _shared_buffer_realloc_function(GUINT buffer_handle,GUINT oldsize,int old_usage,GUINT newsize,int new_usage)
248{
249 return 0;
250}
251
252void _shared_buffer_free_function(GUINT buffer_handle,GUINT size)
253{
254}
255
256//!** Buffer manager operations
257void gim_create_buffer_manager(GBUFFER_MANAGER_PROTOTYPE * prototype,GUINT buffer_manager_id)
258{
259 GBUFFER_MANAGER_DATA * bm_data;
260 bm_data = &g_buffer_managers[buffer_manager_id];
261
262 if(bm_data->m_active==0)
263 {
264 if(g_buffer_managers_count<=buffer_manager_id)
265 {
266 g_buffer_managers_count = buffer_manager_id+1;
267 }
268 }
269 else
270 {
271 gim_destroy_buffer_manager(buffer_manager_id);
272 }
273 bm_data->m_active = 1;
274 //CREATE ARRAYS
275 GIM_DYNARRAY_CREATE(GBUFFER_DATA,bm_data->m_buffer_array,G_ARRAY_GROW_SIZE);
276 GIM_DYNARRAY_CREATE(GUINT,bm_data->m_free_positions,G_ARRAY_GROW_SIZE);
277 //INIT PROTOTYPE
278 bm_data->m_prototype.alloc_data_fn = prototype->alloc_data_fn;
279 bm_data->m_prototype.alloc_fn = prototype->alloc_fn;
280 bm_data->m_prototype.copy_buffers_fn = prototype->copy_buffers_fn;
281 bm_data->m_prototype.download_from_buffer_fn = prototype->download_from_buffer_fn;
282 bm_data->m_prototype.free_fn = prototype->free_fn;
283 bm_data->m_prototype.lock_buffer_fn = prototype->lock_buffer_fn;
284 bm_data->m_prototype.realloc_fn = prototype->realloc_fn;
285 bm_data->m_prototype.unlock_buffer_fn = prototype->unlock_buffer_fn;
286 bm_data->m_prototype.upload_to_buffer_fn = prototype->upload_to_buffer_fn;
287}
288
289GUINT gim_get_buffer_manager_count()
290{
291 return g_buffer_managers_count;
292}
293void gim_destroy_buffer_manager(GUINT buffer_manager_id)
294{
295 GBUFFER_MANAGER_DATA * bm_data;
296 gim_get_buffer_manager_data(buffer_manager_id,&bm_data);
297 if(bm_data == 0) return;
298 //Destroy all buffers
299
300 GBUFFER_DATA * buffers = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
301 GUINT i, buffer_count = bm_data->m_buffer_array.m_size;
302 for (i=0;i<buffer_count ;i++ )
303 {
304 if(buffers[i].m_buffer_handle!=0) //Is active
305 {
306 // free handle
307 bm_data->m_prototype.free_fn(buffers[i].m_buffer_handle,buffers[i].m_size);
308 }
309 }
310
311 //destroy buffer array
312 GIM_DYNARRAY_DESTROY(bm_data->m_buffer_array);
313 //destroy free positions
314 GIM_DYNARRAY_DESTROY(bm_data->m_free_positions);
315 //Mark as innactive
316 bm_data->m_active = 0;
317}
318void gim_get_buffer_manager_data(GUINT buffer_manager_id,GBUFFER_MANAGER_DATA ** pbm_data)
319{
320 GBUFFER_MANAGER_DATA * bm_data;
321 bm_data = &g_buffer_managers[buffer_manager_id];
322
323 if(bm_data->m_active==0)
324 {
325 *pbm_data = 0;
326 }
327 else
328 {
329 *pbm_data = bm_data;
330 }
331}
332
333void gim_init_buffer_managers()
334{
335 GUINT i;
336 for (i=0;i<MAX_BUFFER_MANAGERS;i++)
337 {
338 g_buffer_managers[i].m_active = 0;
339 g_buffer_managers[i].m_buffer_array.m_pdata = 0;
340 g_buffer_managers[i].m_buffer_array.m_reserve_size = 0;
341 g_buffer_managers[i].m_buffer_array.m_size = 0;
342 g_buffer_managers[i].m_free_positions.m_pdata = 0;
343 g_buffer_managers[i].m_free_positions.m_reserve_size = 0;
344 g_buffer_managers[i].m_free_positions.m_size = 0;
345 }
346 g_buffer_managers_count = 0;
347 // Add the two most important buffer managers
348 GBUFFER_MANAGER_PROTOTYPE prototype;
349
350 //add system buffer manager
351 prototype.alloc_data_fn = _system_buffer_alloc_data_function;
352 prototype.alloc_fn = _system_buffer_alloc_function;
353 prototype.copy_buffers_fn = _system_copy_buffers_function;
354 prototype.download_from_buffer_fn = _system_download_from_buffer_function;
355 prototype.free_fn = _system_buffer_free_function;
356 prototype.lock_buffer_fn = _system_lock_buffer_function;
357 prototype.realloc_fn = _system_buffer_realloc_function;
358 prototype.unlock_buffer_fn = _system_unlock_buffer_function;
359 prototype.upload_to_buffer_fn = _system_upload_to_buffer_function;
360
361 gim_create_buffer_manager(&prototype,G_BUFFER_MANAGER_SYSTEM );
362
363 //add zhared buffer manager
364 prototype.alloc_data_fn = _shared_buffer_alloc_data_function;
365 prototype.alloc_fn = _shared_buffer_alloc_function;
366 prototype.free_fn = _shared_buffer_free_function;
367 gim_create_buffer_manager(&prototype,G_BUFFER_MANAGER_SHARED);
368}
369
370void gim_terminate_buffer_managers()
371{
372 GUINT i;
373 for (i=0;i<g_buffer_managers_count;i++)
374 {
375 gim_destroy_buffer_manager(i);
376 }
377 g_buffer_managers_count = 0;
378}
379
380//!** Nuffer operations
381
382void GET_AVALIABLE_BUFFER_ID(GBUFFER_MANAGER_DATA * buffer_manager, GUINT & buffer_id)
383{
384 if(buffer_manager->m_free_positions.m_size>0)\
385 {
386 GUINT * _pointer = GIM_DYNARRAY_POINTER(GUINT,buffer_manager->m_free_positions);
387 buffer_id = _pointer[buffer_manager->m_free_positions.m_size-1];
388 GIM_DYNARRAY_POP_ITEM(buffer_manager->m_free_positions);
389 }
390 else
391 {
392 buffer_id = buffer_manager->m_buffer_array.m_size;
393 GIM_DYNARRAY_PUSH_EMPTY(GBUFFER_DATA,buffer_manager->m_buffer_array);
394 }
395}
396
397GINT _validate_buffer_id(GBUFFER_ID * buffer_id,GBUFFER_DATA ** ppbuffer,GBUFFER_MANAGER_DATA ** pbm_data)
398{
399 VALIDATE_BUFFER_ID_PT(buffer_id)
400 *ppbuffer = pbuffer;
401 *pbm_data = bm_data;
402 return G_BUFFER_OP_SUCCESS;
403}
404
405GUINT gim_create_buffer(
406 GUINT buffer_manager_id,
407 GUINT buffer_size,
408 int usage,
409 GBUFFER_ID * buffer_id)
410{
411 VALIDATE_BUFFER_MANAGER(buffer_manager_id)
412
413 GUINT newbufferhandle = bm_data->m_prototype.alloc_fn(buffer_size,usage);
414 if(newbufferhandle==0) return G_BUFFER_OP_INVALID;
415
416 GET_AVALIABLE_BUFFER_ID(bm_data,buffer_id->m_buffer_id);
417 buffer_id->m_buffer_manager_id = buffer_manager_id;
418
419 GBUFFER_DATA * pbuffer = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
420 pbuffer += buffer_id->m_buffer_id ;
421 pbuffer->m_buffer_handle = newbufferhandle;
422 pbuffer->m_size = buffer_size;
423 pbuffer->m_usage = usage;
424 pbuffer->m_lock_count = 0;
425 pbuffer->m_refcount = 0;
426 pbuffer->m_mapped_pointer = 0;
427
428 //set shadow buffer if needed
429
430 if(usage == G_MU_STATIC_READ ||
431 usage == G_MU_STATIC_READ_DYNAMIC_WRITE||
432 usage == G_MU_STATIC_READ_DYNAMIC_WRITE_COPY)
433 {
434 gim_create_common_buffer(buffer_size,&pbuffer->m_shadow_buffer);
435 }
436 else
437 {
438 pbuffer->m_shadow_buffer.m_buffer_id = G_UINT_INFINITY;
439 pbuffer->m_shadow_buffer.m_buffer_manager_id = G_UINT_INFINITY;
440 }
441 return G_BUFFER_OP_SUCCESS;
442}
443
444
445GUINT gim_create_buffer_from_data(
446 GUINT buffer_manager_id,
447 const void * pdata,
448 GUINT buffer_size,
449 int usage,
450 GBUFFER_ID * buffer_id)
451{
452 VALIDATE_BUFFER_MANAGER(buffer_manager_id)
453
454 GUINT newbufferhandle = bm_data->m_prototype.alloc_data_fn(pdata,buffer_size,usage);
455 if(newbufferhandle==0) return G_BUFFER_OP_INVALID;
456
457 GET_AVALIABLE_BUFFER_ID(bm_data,buffer_id->m_buffer_id);
458 buffer_id->m_buffer_manager_id = buffer_manager_id;
459
460 GBUFFER_DATA * pbuffer = GIM_DYNARRAY_POINTER(GBUFFER_DATA,bm_data->m_buffer_array);
461 pbuffer += buffer_id->m_buffer_id ;
462 pbuffer->m_buffer_handle = newbufferhandle;
463 pbuffer->m_size = buffer_size;
464 pbuffer->m_usage = usage;
465 pbuffer->m_lock_count = 0;
466 pbuffer->m_mapped_pointer = 0;
467 pbuffer->m_refcount = 0;
468
469 //set shadow buffer if needed
470
471 if(usage == G_MU_STATIC_READ ||
472 usage == G_MU_STATIC_READ_DYNAMIC_WRITE||
473 usage == G_MU_STATIC_READ_DYNAMIC_WRITE_COPY)
474 {
475 gim_create_common_buffer_from_data(pdata,buffer_size,&pbuffer->m_shadow_buffer);
476 }
477 else
478 {
479 pbuffer->m_shadow_buffer.m_buffer_id = G_UINT_INFINITY;
480 pbuffer->m_shadow_buffer.m_buffer_manager_id = G_UINT_INFINITY;
481 }
482 return G_BUFFER_OP_SUCCESS;
483}
484
485GUINT gim_create_common_buffer(GUINT buffer_size, GBUFFER_ID * buffer_id)
486{
487 return gim_create_buffer(G_BUFFER_MANAGER_SYSTEM,buffer_size,G_MU_DYNAMIC_READ_WRITE,buffer_id);
488}
489
490GUINT gim_create_common_buffer_from_data(
491 const void * pdata, GUINT buffer_size, GBUFFER_ID * buffer_id)
492{
493 return gim_create_buffer_from_data(G_BUFFER_MANAGER_SYSTEM,pdata,buffer_size,G_MU_DYNAMIC_READ_WRITE,buffer_id);
494}
495
496GUINT gim_create_shared_buffer_from_data(
497 const void * pdata, GUINT buffer_size, GBUFFER_ID * buffer_id)
498{
499 return gim_create_buffer_from_data(G_BUFFER_MANAGER_SHARED,pdata,buffer_size,G_MU_DYNAMIC_READ_WRITE,buffer_id);
500}
501
502GINT gim_buffer_realloc(GBUFFER_ID * buffer_id,GUINT newsize)
503{
504 VALIDATE_BUFFER_ID_PT(buffer_id)
505 if(pbuffer->m_lock_count>0) return G_BUFFER_OP_INVALID;
506 GUINT newhandle = bm_data->m_prototype.realloc_fn(pbuffer->m_buffer_handle,pbuffer->m_size,pbuffer->m_usage,newsize,pbuffer->m_usage);
507 if(newhandle==0) return G_BUFFER_OP_INVALID;
508 pbuffer->m_buffer_handle = newhandle;
509 //realloc shadow buffer if any
510 gim_buffer_realloc(&pbuffer->m_shadow_buffer,newsize);
511 return G_BUFFER_OP_SUCCESS;
512}
513
514GINT gim_buffer_add_ref(GBUFFER_ID * buffer_id)
515{
516 VALIDATE_BUFFER_ID_PT(buffer_id)
517 pbuffer->m_refcount++;
518 return G_BUFFER_OP_SUCCESS;
519}
520
521GINT gim_buffer_free(GBUFFER_ID * buffer_id)
522{
523 VALIDATE_BUFFER_ID_PT(buffer_id)
524 if(pbuffer->m_lock_count>0) return G_BUFFER_OP_INVALID;
525 if(pbuffer->m_refcount>0) pbuffer->m_refcount--;
526 if(pbuffer->m_refcount>0) return G_BUFFER_OP_STILLREFCOUNTED;
527
528 bm_data->m_prototype.free_fn(pbuffer->m_buffer_handle,pbuffer->m_size);
529 //destroy shadow buffer if needed
530 gim_buffer_free(&pbuffer->m_shadow_buffer);
531 // Obtain a free slot index for a new buffer
532 GIM_DYNARRAY_PUSH_ITEM(GUINT,bm_data->m_free_positions,buffer_id->m_buffer_id);
533 pbuffer->m_buffer_handle = 0;
534 pbuffer->m_size = 0;
535 pbuffer->m_shadow_buffer.m_buffer_id = G_UINT_INFINITY;
536 pbuffer->m_shadow_buffer.m_buffer_manager_id = G_UINT_INFINITY;
537 return G_BUFFER_OP_SUCCESS;
538}
539
540GINT gim_lock_buffer(GBUFFER_ID * buffer_id,int access,char ** map_pointer)
541{
542 VALIDATE_BUFFER_ID_PT(buffer_id)
543 if(pbuffer->m_lock_count>0)
544 {
545 if(pbuffer->m_access!=access) return G_BUFFER_OP_INVALID;
546 pbuffer->m_lock_count++;
547 *map_pointer = pbuffer->m_mapped_pointer;
548 return G_BUFFER_OP_SUCCESS;
549 }
550
551 pbuffer->m_access = access;
552
553 GUINT result;
554 if(pbuffer->m_usage==G_MU_STATIC_WRITE)
555 {
556 *map_pointer = 0;///no access
557 return G_BUFFER_OP_INVALID;
558 }
559 else if(pbuffer->m_usage==G_MU_STATIC_READ)
560 {
561 if(pbuffer->m_access == G_MA_READ_ONLY)
562 {
563 result = gim_lock_buffer(&pbuffer->m_shadow_buffer,access,map_pointer);
564 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
565 pbuffer->m_mapped_pointer = *map_pointer;
566 pbuffer->m_lock_count++;
567 }
568 else
569 {
570 *map_pointer = 0;
571 return G_BUFFER_OP_INVALID;
572 }
573 }
574 else if(pbuffer->m_usage==G_MU_STATIC_READ_DYNAMIC_WRITE)
575 {
576 if(pbuffer->m_access == G_MA_READ_ONLY)
577 {
578 result = gim_lock_buffer(&pbuffer->m_shadow_buffer,access,map_pointer);
579 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
580 pbuffer->m_mapped_pointer = *map_pointer;
581 pbuffer->m_lock_count++;
582 }
583 else if(pbuffer->m_access == G_MA_WRITE_ONLY)
584 {
585 pbuffer->m_mapped_pointer = bm_data->m_prototype.lock_buffer_fn(pbuffer->m_buffer_handle,access);
586 *map_pointer = pbuffer->m_mapped_pointer;
587 pbuffer->m_lock_count++;
588 }
589 else if(pbuffer->m_access == G_MA_READ_WRITE)
590 {
591 *map_pointer = 0;
592 return G_BUFFER_OP_INVALID;
593 }
594 }
595 else if(pbuffer->m_usage==G_MU_STATIC_READ_DYNAMIC_WRITE_COPY)
596 {
597 result = gim_lock_buffer(&pbuffer->m_shadow_buffer,access,map_pointer);
598 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
599 pbuffer->m_mapped_pointer = *map_pointer;
600 pbuffer->m_lock_count++;
601 }
602 else if(pbuffer->m_usage==G_MU_STATIC_WRITE_DYNAMIC_READ)
603 {
604 if(pbuffer->m_access == G_MA_READ_ONLY)
605 {
606 pbuffer->m_mapped_pointer = bm_data->m_prototype.lock_buffer_fn(pbuffer->m_buffer_handle,access);
607 *map_pointer = pbuffer->m_mapped_pointer;
608 pbuffer->m_lock_count++;
609 }
610 else
611 {
612 *map_pointer = 0;
613 return G_BUFFER_OP_INVALID;
614 }
615 }
616 else if(pbuffer->m_usage==G_MU_DYNAMIC_READ_WRITE)
617 {
618 pbuffer->m_mapped_pointer = bm_data->m_prototype.lock_buffer_fn(pbuffer->m_buffer_handle,access);
619 *map_pointer = pbuffer->m_mapped_pointer;
620 pbuffer->m_lock_count++;
621 }
622 return G_BUFFER_OP_SUCCESS;
623}
624
625GINT gim_unlock_buffer(GBUFFER_ID * buffer_id)
626{
627 VALIDATE_BUFFER_ID_PT(buffer_id)
628 if(pbuffer->m_lock_count==0) return G_BUFFER_OP_INVALID;
629
630 if(pbuffer->m_lock_count>1)
631 {
632 pbuffer->m_lock_count--;
633 return G_BUFFER_OP_SUCCESS;
634 }
635
636
637 GUINT result;
638 if(pbuffer->m_usage==G_MU_STATIC_WRITE)
639 {
640 pbuffer->m_mapped_pointer = 0;
641 pbuffer->m_lock_count=0;
642 return G_BUFFER_OP_INVALID;
643 }
644 else if(pbuffer->m_usage==G_MU_STATIC_READ)
645 {
646 if(pbuffer->m_access == G_MA_READ_ONLY)
647 {
648 result = gim_unlock_buffer(&pbuffer->m_shadow_buffer);
649 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
650 pbuffer->m_mapped_pointer = 0;
651 pbuffer->m_lock_count=0;
652 }
653 else
654 {
655 pbuffer->m_mapped_pointer = 0;
656 pbuffer->m_lock_count=0;
657 return G_BUFFER_OP_INVALID;
658 }
659 }
660 else if(pbuffer->m_usage==G_MU_STATIC_READ_DYNAMIC_WRITE)
661 {
662 if(pbuffer->m_access == G_MA_READ_ONLY)
663 {
664 result = gim_unlock_buffer(&pbuffer->m_shadow_buffer);
665 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
666 pbuffer->m_mapped_pointer = 0;
667 pbuffer->m_lock_count=0;
668 }
669 else if(pbuffer->m_access == G_MA_WRITE_ONLY)
670 {
671 bm_data->m_prototype.unlock_buffer_fn(pbuffer->m_buffer_handle);
672 pbuffer->m_mapped_pointer = 0;
673 pbuffer->m_lock_count=0;
674 }
675 else if(pbuffer->m_access == G_MA_READ_WRITE)
676 {
677 pbuffer->m_mapped_pointer = 0;
678 pbuffer->m_lock_count=0;
679 return G_BUFFER_OP_INVALID;
680 }
681 }
682 else if(pbuffer->m_usage==G_MU_STATIC_READ_DYNAMIC_WRITE_COPY)
683 {
684 result = gim_unlock_buffer(&pbuffer->m_shadow_buffer);
685 if(result!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
686 pbuffer->m_mapped_pointer = 0;
687 pbuffer->m_lock_count=0;
688 if(pbuffer->m_access == G_MA_WRITE_ONLY||pbuffer->m_access == G_MA_READ_WRITE)
689 {
690 gim_copy_buffers(&pbuffer->m_shadow_buffer,0,buffer_id,0,pbuffer->m_size);
691 }
692 }
693 else if(pbuffer->m_usage==G_MU_STATIC_WRITE_DYNAMIC_READ)
694 {
695 if(pbuffer->m_access == G_MA_READ_ONLY)
696 {
697 bm_data->m_prototype.unlock_buffer_fn(pbuffer->m_buffer_handle);
698 pbuffer->m_mapped_pointer = 0;
699 pbuffer->m_lock_count=0;
700 }
701 else
702 {
703 pbuffer->m_mapped_pointer = 0;
704 pbuffer->m_lock_count=0;
705 return G_BUFFER_OP_INVALID;
706 }
707 }
708 else if(pbuffer->m_usage==G_MU_DYNAMIC_READ_WRITE)
709 {
710 bm_data->m_prototype.unlock_buffer_fn(pbuffer->m_buffer_handle);
711 pbuffer->m_mapped_pointer = 0;
712 pbuffer->m_lock_count=0;
713 }
714 return G_BUFFER_OP_SUCCESS;
715}
716
717GINT gim_get_buffer_size(GBUFFER_ID * buffer_id,GUINT * buffer_size)
718{
719 VALIDATE_BUFFER_ID_PT(buffer_id)
720 *buffer_size = pbuffer->m_size;
721 return G_BUFFER_OP_SUCCESS;
722}
723
724GINT gim_get_buffer_is_locked(GBUFFER_ID * buffer_id,GUINT * lock_count)
725{
726 VALIDATE_BUFFER_ID_PT(buffer_id)
727 *lock_count = pbuffer->m_lock_count;
728 return G_BUFFER_OP_SUCCESS;
729}
730
731
732GINT gim_download_from_buffer(
733 GBUFFER_ID * buffer_id,
734 GUINT source_pos,
735 void * destdata,
736 GUINT copysize)
737{
738 VALIDATE_BUFFER_ID_PT(buffer_id)
739 bm_data->m_prototype.download_from_buffer_fn(
740 pbuffer->m_buffer_handle,source_pos,destdata,copysize);
741 return G_BUFFER_OP_SUCCESS;
742}
743
744GINT gim_upload_to_buffer(
745 GBUFFER_ID * buffer_id,
746 GUINT dest_pos,
747 void * sourcedata,
748 GUINT copysize)
749{
750 VALIDATE_BUFFER_ID_PT(buffer_id)
751 bm_data->m_prototype.upload_to_buffer_fn(
752 pbuffer->m_buffer_handle,dest_pos,sourcedata,copysize);
753 return G_BUFFER_OP_SUCCESS;
754}
755
756GINT gim_copy_buffers(
757 GBUFFER_ID * source_buffer_id,
758 GUINT source_pos,
759 GBUFFER_ID * dest_buffer_id,
760 GUINT dest_pos,
761 GUINT copysize)
762{
763 GBUFFER_MANAGER_DATA * bm_data1,* bm_data2;
764 GBUFFER_DATA * pbuffer1, * pbuffer2;
765 void * tempdata;
766 if(_validate_buffer_id(source_buffer_id,&pbuffer1,&bm_data1)!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
767
768 if(_validate_buffer_id(dest_buffer_id,&pbuffer2,&bm_data2)!= G_BUFFER_OP_SUCCESS) return G_BUFFER_OP_INVALID;
769
770 if((source_buffer_id->m_buffer_manager_id == dest_buffer_id->m_buffer_manager_id)||
771 (source_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SYSTEM && dest_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SHARED)||
772 (source_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SHARED && dest_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SYSTEM)
773 )
774 {//smooth copy
775 bm_data1->m_prototype.copy_buffers_fn(pbuffer1->m_buffer_handle,source_pos,pbuffer2->m_buffer_handle,dest_pos,copysize);
776 }
777 else if(source_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SYSTEM || source_buffer_id->m_buffer_manager_id == G_BUFFER_MANAGER_SHARED)
778 {
779 //hard copy
780 tempdata = (void *)pbuffer1->m_buffer_handle;
781 //upload data
782 bm_data2->m_prototype.upload_to_buffer_fn(pbuffer2->m_buffer_handle,dest_pos,
783 tempdata,
784 copysize);
785 }
786 else
787 {
788 //very hard copy
789 void * tempdata = gim_alloc(copysize);
790 //download data
791 bm_data1->m_prototype.download_from_buffer_fn(pbuffer1->m_buffer_handle,source_pos,
792 tempdata,
793 copysize);
794
795 //upload data
796 bm_data2->m_prototype.upload_to_buffer_fn(pbuffer2->m_buffer_handle,dest_pos,
797 tempdata,
798 copysize);
799 //delete temp buffer
800 gim_free(tempdata,copysize);
801 }
802 return G_BUFFER_OP_SUCCESS;
803}
804
805GINT gim_buffer_array_lock(GBUFFER_ARRAY * array_data, int access)
806{
807 if(array_data->m_buffer_data != 0) return G_BUFFER_OP_SUCCESS;
808 GINT result = gim_lock_buffer(&array_data->m_buffer_id,access,&array_data->m_buffer_data);
809 if(result!= G_BUFFER_OP_SUCCESS) return result;
810 array_data->m_buffer_data += array_data->m_byte_offset;
811 return result;
812}
813
814GINT gim_buffer_array_unlock(GBUFFER_ARRAY * array_data)
815{
816 if(array_data->m_buffer_data == 0) return G_BUFFER_OP_SUCCESS;
817 GINT result = gim_unlock_buffer(&array_data->m_buffer_id);
818 if(result!= G_BUFFER_OP_SUCCESS) return result;
819 array_data->m_buffer_data = 0;
820 return result;
821}
822
823void gim_buffer_array_copy_ref(GBUFFER_ARRAY * source_data,GBUFFER_ARRAY * dest_data)
824{
825 dest_data->m_buffer_id.m_buffer_id = source_data->m_buffer_id.m_buffer_id;
826 dest_data->m_buffer_id.m_buffer_manager_id = source_data->m_buffer_id.m_buffer_manager_id;
827 dest_data->m_buffer_data = 0;
828 dest_data->m_byte_stride = source_data->m_byte_stride;
829 dest_data->m_byte_offset = source_data->m_byte_offset;
830 dest_data->m_element_count = source_data->m_element_count;
831 gim_buffer_add_ref(&dest_data->m_buffer_id);
832}
833
834void gim_buffer_array_copy_value(GBUFFER_ARRAY * source_data,GBUFFER_ARRAY * dest_data, GUINT buffer_manager_id,int usage)
835{
836 //Create new buffer
837 GUINT buffsize = source_data->m_element_count*source_data->m_byte_stride;
838 gim_create_buffer(buffer_manager_id,buffsize,usage,&dest_data->m_buffer_id);
839
840 //copy ref data
841 dest_data->m_buffer_data = 0;
842 dest_data->m_byte_stride = source_data->m_byte_stride;
843 dest_data->m_byte_offset = 0;
844 dest_data->m_element_count = source_data->m_element_count;
845 gim_buffer_add_ref(&dest_data->m_buffer_id);
846 //copy buffers
847 gim_copy_buffers(&source_data->m_buffer_id,source_data->m_byte_offset,&dest_data->m_buffer_id,0,buffsize);
848}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_tri_tri_overlap.cpp b/libraries/ode-0.9/GIMPACT/src/gim_tri_tri_overlap.cpp
new file mode 100644
index 0000000..5b4e08d
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_tri_tri_overlap.cpp
@@ -0,0 +1,251 @@
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
32#define FABS(x) (float(fabs(x))) /* implement as is fastest on your machine */
33
34/* some macros */
35
36#define CLASSIFY_TRIPOINTS_BY_FACE(v1,v2,v3,faceplane,out_of_face)\
37{ \
38 _distances[0] = DISTANCE_PLANE_POINT(faceplane,v1);\
39 _distances[1] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v2);\
40 _distances[2] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v3); \
41 if(_distances[1]>0.0f && _distances[2]>0.0f)\
42 {\
43 out_of_face = 1;\
44 }\
45 else\
46 {\
47 out_of_face = 0;\
48 }\
49}\
50
51/* sort so that a<=b */
52#define SORT(a,b) \
53 if(a>b) \
54 { \
55 float c; \
56 c=a; \
57 a=b; \
58 b=c; \
59 }
60
61
62/* this edge to edge test is based on Franlin Antonio's gem:
63 "Faster Line Segment Intersection", in Graphics Gems III,
64 pp. 199-202 */
65#define EDGE_EDGE_TEST(V0,U0,U1) \
66 Bx=U0[i0]-U1[i0]; \
67 By=U0[i1]-U1[i1]; \
68 Cx=V0[i0]-U0[i0]; \
69 Cy=V0[i1]-U0[i1]; \
70 f=Ay*Bx-Ax*By; \
71 d=By*Cx-Bx*Cy; \
72 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
73 { \
74 e=Ax*Cy-Ay*Cx; \
75 if(f>0) \
76 { \
77 if(e>=0 && e<=f) return 1; \
78 } \
79 else \
80 { \
81 if(e<=0 && e>=f) return 1; \
82 } \
83 }
84
85#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
86{ \
87 float Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
88 Ax=V1[i0]-V0[i0]; \
89 Ay=V1[i1]-V0[i1]; \
90 /* test edge U0,U1 against V0,V1 */ \
91 EDGE_EDGE_TEST(V0,U0,U1); \
92 /* test edge U1,U2 against V0,V1 */ \
93 EDGE_EDGE_TEST(V0,U1,U2); \
94 /* test edge U2,U1 against V0,V1 */ \
95 EDGE_EDGE_TEST(V0,U2,U0); \
96}
97
98#define POINT_IN_TRI(V0,U0,U1,U2) \
99{ \
100 float a,b,c,d0,d1,d2; \
101 /* is T1 completly inside T2? */ \
102 /* check if V0 is inside tri(U0,U1,U2) */ \
103 a=U1[i1]-U0[i1]; \
104 b=-(U1[i0]-U0[i0]); \
105 c=-a*U0[i0]-b*U0[i1]; \
106 d0=a*V0[i0]+b*V0[i1]+c; \
107 \
108 a=U2[i1]-U1[i1]; \
109 b=-(U2[i0]-U1[i0]); \
110 c=-a*U1[i0]-b*U1[i1]; \
111 d1=a*V0[i0]+b*V0[i1]+c; \
112 \
113 a=U0[i1]-U2[i1]; \
114 b=-(U0[i0]-U2[i0]); \
115 c=-a*U2[i0]-b*U2[i1]; \
116 d2=a*V0[i0]+b*V0[i1]+c; \
117 if(d0*d1>0.0) \
118 { \
119 if(d0*d2>0.0) return 1; \
120 } \
121}
122
123int coplanar_tri_tri(GIM_TRIANGLE_DATA *tri1,
124 GIM_TRIANGLE_DATA *tri2)
125{
126 short i0,i1;
127 /* first project onto an axis-aligned plane, that maximizes the area */
128 /* of the triangles, compute indices: i0,i1. */
129 PLANE_MINOR_AXES(tri1->m_planes.m_planes[0], i0, i1);
130
131 /* test all edges of triangle 1 against the edges of triangle 2 */
132 EDGE_AGAINST_TRI_EDGES(tri1->m_vertices[0],tri1->m_vertices[1],tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2]);
133 EDGE_AGAINST_TRI_EDGES(tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2]);
134 EDGE_AGAINST_TRI_EDGES(tri1->m_vertices[2],tri1->m_vertices[0],tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2]);
135
136 /* finally, test if tri1 is totally contained in tri2 or vice versa */
137 POINT_IN_HULL(tri1->m_vertices[0],(&tri2->m_planes.m_planes[1]),3,i0);
138 if(i0==0) return 1;
139
140 POINT_IN_HULL(tri2->m_vertices[0],(&tri1->m_planes.m_planes[1]),3,i0);
141 if(i0==0) return 1;
142
143 return 0;
144}
145
146
147
148#define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
149{ \
150 if(D0D1>0.0f) \
151 { \
152 /* here we know that D0D2<=0.0 */ \
153 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
154 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
155 } \
156 else if(D0D2>0.0f)\
157 { \
158 /* here we know that d0d1<=0.0 */ \
159 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
160 } \
161 else if(D1*D2>0.0f || D0!=0.0f) \
162 { \
163 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
164 A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
165 } \
166 else if(D1!=0.0f) \
167 { \
168 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
169 } \
170 else if(D2!=0.0f) \
171 { \
172 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
173 } \
174 else \
175 { \
176 /* triangles are coplanar */ \
177 return coplanar_tri_tri(tri1,tri2); \
178 } \
179}\
180
181
182
183int gim_triangle_triangle_overlap(
184 GIM_TRIANGLE_DATA *tri1,
185 GIM_TRIANGLE_DATA *tri2)
186{
187 vec3f _distances;
188 char out_of_face;
189 CLASSIFY_TRIPOINTS_BY_FACE(tri1->m_vertices[0],tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_planes.m_planes[0],out_of_face);
190 if(out_of_face==1) return 0;
191
192 CLASSIFY_TRIPOINTS_BY_FACE(tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2],tri1->m_planes.m_planes[0],out_of_face);
193 if(out_of_face==1) return 0;
194
195
196 float du0=0,du1=0,du2=0,dv0=0,dv1=0,dv2=0;
197 float D[3];
198 float isect1[2], isect2[2];
199 float du0du1=0,du0du2=0,dv0dv1=0,dv0dv2=0;
200 short index;
201 float vp0,vp1,vp2;
202 float up0,up1,up2;
203 float bb,cc,max;
204
205 /* compute direction of intersection line */
206 VEC_CROSS(D,tri1->m_planes.m_planes[0],tri2->m_planes.m_planes[0]);
207
208 /* compute and index to the largest component of D */
209 max=(float)FABS(D[0]);
210 index=0;
211 bb=(float)FABS(D[1]);
212 cc=(float)FABS(D[2]);
213 if(bb>max) max=bb,index=1;
214 if(cc>max) max=cc,index=2;
215
216 /* this is the simplified projection onto L*/
217 vp0= tri1->m_vertices[0][index];
218 vp1= tri1->m_vertices[1][index];
219 vp2= tri1->m_vertices[2][index];
220
221 up0= tri2->m_vertices[0][index];
222 up1= tri2->m_vertices[1][index];
223 up2= tri2->m_vertices[2][index];
224
225 /* compute interval for triangle 1 */
226 float a,b,c,x0,x1;
227 NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
228
229 /* compute interval for triangle 2 */
230 float d,e,f,y0,y1;
231 NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
232
233 float xx,yy,xxyy,tmp;
234 xx=x0*x1;
235 yy=y0*y1;
236 xxyy=xx*yy;
237
238 tmp=a*xxyy;
239 isect1[0]=tmp+b*x1*yy;
240 isect1[1]=tmp+c*x0*yy;
241
242 tmp=d*xxyy;
243 isect2[0]=tmp+e*xx*y1;
244 isect2[1]=tmp+f*xx*y0;
245
246 SORT(isect1[0],isect1[1]);
247 SORT(isect2[0],isect2[1]);
248
249 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
250 return 1;
251}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_trimesh.cpp b/libraries/ode-0.9/GIMPACT/src/gim_trimesh.cpp
new file mode 100644
index 0000000..1872592
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_trimesh.cpp
@@ -0,0 +1,364 @@
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
30#include <assert.h>
31#include "GIMPACT/gim_trimesh.h"
32
33GUINT gim_trimesh_get_triangle_count(GIM_TRIMESH * trimesh)
34{
35 return trimesh->m_tri_index_buffer.m_element_count/3;
36}
37
38//! Creates the aabb set and the triangles cache
39/*!
40
41\param trimesh
42\param vertex_array
43\param triindex_array
44\param transformed_reply If 1, then the m_transformed_vertices is a reply of the source vertices. Else it just be a reference to the original array.
45\post it copies the arrays by reference, and creates the auxiliary data (m_aabbset,m_planes_cache_buffer)
46*/
47void gim_trimesh_create_from_arrays(GIM_TRIMESH * trimesh, GBUFFER_ARRAY * vertex_array, GBUFFER_ARRAY * triindex_array,char transformed_reply)
48{
49 assert(trimesh);
50 assert(vertex_array);
51 assert(triindex_array);
52 gim_buffer_array_copy_ref(vertex_array,&trimesh->m_source_vertex_buffer);
53 gim_buffer_array_copy_ref(triindex_array,&trimesh->m_tri_index_buffer);
54
55 trimesh->m_mask = GIM_TRIMESH_NEED_UPDATE;//needs update
56 //Create the transformed vertices
57 if(transformed_reply==1)
58 {
59 trimesh->m_mask |= GIM_TRIMESH_TRANSFORMED_REPLY;
60 gim_buffer_array_copy_value(vertex_array,&trimesh->m_transformed_vertex_buffer,G_BUFFER_MANAGER_SYSTEM,G_MU_DYNAMIC_READ_WRITE);
61 }
62 else
63 {
64 gim_buffer_array_copy_ref(vertex_array,&trimesh->m_transformed_vertex_buffer);
65 }
66 //create the box set
67 GUINT facecount = gim_trimesh_get_triangle_count(trimesh);
68
69 gim_aabbset_alloc(&trimesh->m_aabbset,facecount);
70 //create the planes cache
71 GIM_DYNARRAY_CREATE_SIZED(GIM_TRIPLANES_CACHE,trimesh->m_planes_cache_buffer,facecount);
72 //Create the bitset
73 GIM_BITSET_CREATE_SIZED(trimesh->m_planes_cache_bitset,facecount);
74 //Callback is 0
75 trimesh->m_update_callback = 0;
76 //set to identity
77 IDENTIFY_MATRIX_4X4(trimesh->m_transform);
78}
79
80
81
82//! Create a trimesh from vertex array and an index array
83/*!
84
85\param trimesh An uninitialized GIM_TRIMESH structure
86\param vertex_array A buffer to a vec3f array
87\param vertex_count
88\param triindex_array
89\param index_count
90\param copy_vertices If 1, it copies the source vertices in another buffer. Else (0) it constructs a reference to the data.
91\param copy_indices If 1, it copies the source vertices in another buffer. Else (0) it constructs a reference to the data.
92\param transformed_reply If , then the m_transformed_vertices is a reply of the source vertices. Else it just be a reference to the original array.
93*/
94void gim_trimesh_create_from_data(GIM_TRIMESH * trimesh, vec3f * vertex_array, GUINT vertex_count,char copy_vertices, GUINT * triindex_array, GUINT index_count,char copy_indices,char transformed_reply)
95{
96 GBUFFER_ARRAY buffer_vertex_array;
97 GBUFFER_ARRAY buffer_triindex_array;
98
99 //Create vertices
100 if(copy_vertices == 1)
101 {
102 gim_create_common_buffer_from_data(vertex_array, vertex_count*sizeof(vec3f), &buffer_vertex_array.m_buffer_id);
103 }
104 else//Create a shared buffer
105 {
106 gim_create_shared_buffer_from_data(vertex_array, vertex_count*sizeof(vec3f), &buffer_vertex_array.m_buffer_id);
107 }
108 GIM_BUFFER_ARRAY_INIT_TYPE(vec3f,buffer_vertex_array,buffer_vertex_array.m_buffer_id,vertex_count);
109
110
111 //Create vertices
112 if(copy_indices == 1)
113 {
114 gim_create_common_buffer_from_data(triindex_array, index_count*sizeof(GUINT), &buffer_triindex_array.m_buffer_id);
115 }
116 else//Create a shared buffer
117 {
118 gim_create_shared_buffer_from_data(triindex_array, index_count*sizeof(GUINT), &buffer_triindex_array.m_buffer_id);
119 }
120 GIM_BUFFER_ARRAY_INIT_TYPE(GUINT,buffer_triindex_array,buffer_triindex_array.m_buffer_id,index_count);
121
122 gim_trimesh_create_from_arrays(trimesh, &buffer_vertex_array, &buffer_triindex_array,transformed_reply);
123
124 ///always call this after create a buffer_array
125 GIM_BUFFER_ARRAY_DESTROY(buffer_vertex_array);
126 GIM_BUFFER_ARRAY_DESTROY(buffer_triindex_array);
127}
128
129//! Clears auxiliary data and releases buffer arrays
130void gim_trimesh_destroy(GIM_TRIMESH * trimesh)
131{
132 gim_aabbset_destroy(&trimesh->m_aabbset);
133
134 GIM_DYNARRAY_DESTROY(trimesh->m_planes_cache_buffer);
135 GIM_DYNARRAY_DESTROY(trimesh->m_planes_cache_bitset);
136
137 GIM_BUFFER_ARRAY_DESTROY(trimesh->m_transformed_vertex_buffer);
138 GIM_BUFFER_ARRAY_DESTROY(trimesh->m_source_vertex_buffer);
139 GIM_BUFFER_ARRAY_DESTROY(trimesh->m_tri_index_buffer);
140}
141
142//! Copies two meshes
143/*!
144\pre dest_trimesh shouldn't be created
145\post dest_trimesh will be created
146\param source_trimesh
147\param dest_trimesh
148\param copy_by_reference If 1, it attach a reference to the source vertices, else it copies the vertices
149\param transformed_reply IF 1, then it forces the m_trasnformed_vertices to be a reply of the source vertices
150*/
151void gim_trimesh_copy(GIM_TRIMESH * source_trimesh,GIM_TRIMESH * dest_trimesh, char copy_by_reference, char transformed_reply)
152{
153 if(copy_by_reference==1)
154 {
155 gim_trimesh_create_from_arrays(dest_trimesh, &source_trimesh->m_source_vertex_buffer, &source_trimesh->m_tri_index_buffer,transformed_reply);
156 }
157 else
158 {
159 GBUFFER_ARRAY buffer_vertex_array;
160 GBUFFER_ARRAY buffer_triindex_array;
161
162 gim_buffer_array_copy_value(&source_trimesh->m_source_vertex_buffer,&buffer_vertex_array,G_BUFFER_MANAGER_SYSTEM,G_MU_DYNAMIC_READ_WRITE);
163
164 gim_buffer_array_copy_value(&source_trimesh->m_tri_index_buffer,&buffer_triindex_array,G_BUFFER_MANAGER_SYSTEM,G_MU_DYNAMIC_READ_WRITE);
165
166 gim_trimesh_create_from_arrays(dest_trimesh, &buffer_vertex_array, &buffer_triindex_array,transformed_reply);
167
168 ///always call this after create a buffer_array
169 GIM_BUFFER_ARRAY_DESTROY(buffer_vertex_array);
170 GIM_BUFFER_ARRAY_DESTROY(buffer_triindex_array);
171 }
172}
173
174//! Locks the trimesh for working with it
175/*!
176\post locks m_tri_index_buffer and m_transformed_vertex_buffer.
177\param trimesh
178*/
179void gim_trimesh_locks_work_data(GIM_TRIMESH * trimesh)
180{
181 GINT res;
182 res=gim_buffer_array_lock(&trimesh->m_tri_index_buffer,G_MA_READ_ONLY);
183 assert(res==G_BUFFER_OP_SUCCESS);
184 res=gim_buffer_array_lock(&trimesh->m_transformed_vertex_buffer,G_MA_READ_ONLY);
185 assert(res==G_BUFFER_OP_SUCCESS);
186}
187
188//! unlocks the trimesh
189/*!
190\post unlocks m_tri_index_buffer and m_transformed_vertex_buffer.
191\param trimesh
192*/
193void gim_trimesh_unlocks_work_data(GIM_TRIMESH * trimesh)
194{
195 gim_buffer_array_unlock(&trimesh->m_tri_index_buffer);
196 gim_buffer_array_unlock(&trimesh->m_transformed_vertex_buffer);
197}
198
199
200//! Returns 1 if the m_transformed_vertex_buffer is a reply of m_source_vertex_buffer
201char gim_trimesh_has_tranformed_reply(GIM_TRIMESH * trimesh)
202{
203 if(trimesh->m_mask&GIM_TRIMESH_TRANSFORMED_REPLY) return 1;
204 return 0;
205}
206
207//! Returns 1 if the trimesh needs to update their aabbset and the planes cache.
208char gim_trimesh_needs_update(GIM_TRIMESH * trimesh)
209{
210 if(trimesh->m_mask&GIM_TRIMESH_NEED_UPDATE) return 1;
211 return 0;
212}
213
214//! Change the state of the trimesh for force it to update
215/*!
216Call it after made changes to the trimesh.
217\post gim_trimesh_need_update(trimesh) will return 1
218*/
219void gim_trimesh_post_update(GIM_TRIMESH * trimesh)
220{
221 trimesh->m_mask |= GIM_TRIMESH_NEED_UPDATE;
222}
223
224//kernel
225#define MULT_MAT_VEC4_KERNEL(_mat,_src,_dst) MAT_DOT_VEC_3X4((_dst),(_mat),(_src))
226
227//! Updates m_transformed_vertex_buffer
228/*!
229\pre m_transformed_vertex_buffer must be unlocked
230*/
231void gim_trimesh_update_vertices(GIM_TRIMESH * trimesh)
232{
233 if(gim_trimesh_has_tranformed_reply(trimesh) == 0) return; //Don't perform transformation
234
235 //Vertices
236 GBUFFER_ARRAY * psource_vertex_buffer = &trimesh->m_source_vertex_buffer;
237 GBUFFER_ARRAY * ptransformed_vertex_buffer = &trimesh->m_transformed_vertex_buffer;
238 //Temp transform
239 mat4f transform;
240 COPY_MATRIX_4X4(transform,trimesh->m_transform);
241
242 GIM_PROCESS_BUFFER_ARRAY(transform,(*psource_vertex_buffer),(*ptransformed_vertex_buffer),MULT_MAT_VEC4_KERNEL,vec3f,vec3f);
243}
244
245//! Updates m_aabbset and m_planes_cache_bitset
246/*!
247\pre gim_trimesh_locks_work_data must be called before
248*/
249void gim_trimesh_update_aabbset(GIM_TRIMESH * trimesh)
250{
251 vec3f * transformed_vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
252 assert(transformed_vertices);
253
254 GUINT * triangle_indices = GIM_BUFFER_ARRAY_POINTER(GUINT,trimesh->m_tri_index_buffer,0);
255 assert(triangle_indices);
256 // box set
257 aabb3f * paabb = trimesh->m_aabbset.m_boxes;
258 GUINT triangle_count = gim_trimesh_get_triangle_count(trimesh);
259 float * v1,*v2,*v3;
260 GUINT i;
261 for (i=0; i<triangle_count;i++)
262 {
263 v1 = &transformed_vertices[triangle_indices[0]][0];
264 v2 = &transformed_vertices[triangle_indices[1]][0];
265 v3 = &transformed_vertices[triangle_indices[2]][0];
266 COMPUTEAABB_FOR_TRIANGLE((*paabb),v1,v2,v3);
267 triangle_indices+=3;
268 paabb++;
269 }
270 //Clear planes cache
271 GIM_BITSET_CLEAR_ALL(trimesh->m_planes_cache_bitset);
272 //Sorts set
273 gim_aabbset_update(&trimesh->m_aabbset);
274}
275
276//! Updates the trimesh if needed
277/*!
278\post If gim_trimesh_needs_update returns 1, then it calls gim_trimesh_update_vertices and gim_trimesh_update_aabbset
279*/
280void gim_trimesh_update(GIM_TRIMESH * trimesh)
281{
282 if(gim_trimesh_needs_update(trimesh)==0) return;
283 gim_trimesh_update_vertices(trimesh);
284 gim_trimesh_locks_work_data(trimesh);
285 gim_trimesh_update_aabbset(trimesh);
286 gim_trimesh_unlocks_work_data(trimesh);
287
288 //Clear update flag
289 trimesh->m_mask &= ~GIM_TRIMESH_NEED_UPDATE;
290}
291
292void gim_trimesh_set_tranform(GIM_TRIMESH * trimesh, mat4f transform)
293{
294 GREAL diff = 0.0f;
295 float * originaltrans = &trimesh->m_transform[0][0];
296 float * newtrans = &transform[0][0];
297 GUINT i;
298 for (i=0;i<16;i++)
299 {
300 diff += fabs(originaltrans[i]-newtrans[i]);
301 }
302
303// if(IS_ZERO(diff)) return ;///don't need to update
304 if(diff< 0.00001f) return ;///don't need to update
305
306 COPY_MATRIX_4X4(trimesh->m_transform,transform);
307
308 gim_trimesh_post_update(trimesh);
309}
310
311void gim_trimesh_get_triangle_data(GIM_TRIMESH * trimesh, GUINT triangle_index, GIM_TRIANGLE_DATA * tri_data)
312{
313 vec3f * transformed_vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
314
315 GUINT * triangle_indices = GIM_BUFFER_ARRAY_POINTER(GUINT,trimesh->m_tri_index_buffer,triangle_index*3);
316
317
318 //Copy the vertices
319 VEC_COPY(tri_data->m_vertices[0],transformed_vertices[triangle_indices[0]]);
320 VEC_COPY(tri_data->m_vertices[1],transformed_vertices[triangle_indices[1]]);
321 VEC_COPY(tri_data->m_vertices[2],transformed_vertices[triangle_indices[2]]);
322
323 //Get the planes
324 GIM_TRIPLANES_CACHE * planes = GIM_DYNARRAY_POINTER(GIM_TRIPLANES_CACHE,trimesh->m_planes_cache_buffer);
325 planes += triangle_index;
326
327 //verify planes cache
328 GUINT bit_eval;
329 GIM_BITSET_GET(trimesh->m_planes_cache_bitset,triangle_index,bit_eval);
330 if(bit_eval == 0)// Needs to calc the planes
331 {
332 //Calc the face plane
333 TRIANGLE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],tri_data->m_vertices[2],planes->m_planes[0]);
334 //Calc the edge 1
335 EDGE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],(planes->m_planes[0]),(planes->m_planes[1]));
336
337 //Calc the edge 2
338 EDGE_PLANE(tri_data->m_vertices[1],tri_data->m_vertices[2],(planes->m_planes[0]),(planes->m_planes[2]));
339
340 //Calc the edge 3
341 EDGE_PLANE(tri_data->m_vertices[2],tri_data->m_vertices[0],(planes->m_planes[0]), (planes->m_planes[3]));
342
343 //mark
344 GIM_BITSET_SET(trimesh->m_planes_cache_bitset,triangle_index);
345 }
346
347
348 VEC_COPY_4((tri_data->m_planes.m_planes[0]),(planes->m_planes[0]));//face plane
349 VEC_COPY_4((tri_data->m_planes.m_planes[1]),(planes->m_planes[1]));//edge1
350 VEC_COPY_4((tri_data->m_planes.m_planes[2]),(planes->m_planes[2]));//edge2
351 VEC_COPY_4((tri_data->m_planes.m_planes[3]),(planes->m_planes[3]));//edge3
352}
353
354void gim_trimesh_get_triangle_vertices(GIM_TRIMESH * trimesh, GUINT triangle_index, vec3f v1,vec3f v2,vec3f v3)
355{
356 vec3f * transformed_vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
357
358 GUINT * triangle_indices = GIM_BUFFER_ARRAY_POINTER(GUINT,trimesh->m_tri_index_buffer,triangle_index*3);
359
360 //Copy the vertices
361 VEC_COPY(v1,transformed_vertices[triangle_indices[0]]);
362 VEC_COPY(v2,transformed_vertices[triangle_indices[1]]);
363 VEC_COPY(v3,transformed_vertices[triangle_indices[2]]);
364}
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}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_trimesh_ray_collision.cpp b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_ray_collision.cpp
new file mode 100644
index 0000000..0c10fe1
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_ray_collision.cpp
@@ -0,0 +1,140 @@
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
32//! Trimesh Ray Collisions
33/*!
34
35\param trimesh
36\param contact
37\return 1 if the ray collides, else 0
38*/
39int gim_trimesh_ray_collision(GIM_TRIMESH * trimesh,vec3f origin,vec3f dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA * contact)
40{
41 GDYNAMIC_ARRAY collision_result;
42 GIM_CREATE_BOXQUERY_LIST(collision_result);
43
44 gim_aabbset_ray_collision(origin,dir,tmax,&trimesh->m_aabbset,&collision_result);
45
46 if(collision_result.m_size==0)
47 {
48 GIM_DYNARRAY_DESTROY(collision_result);
49 return 0;
50 }
51
52 //collide triangles
53
54 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
55 GIM_TRIANGLE_DATA tridata;
56 vec3f pout;
57 GREAL tparam,u,v;
58 char does_intersect;
59
60 gim_trimesh_locks_work_data(trimesh);
61
62 for(unsigned int i=0;i<collision_result.m_size;i++)
63 {
64 gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tridata);
65
66 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);
67 if(does_intersect)
68 {
69 contact->tparam = tparam;
70 contact->u = u;
71 contact->v = v;
72 contact->m_face_id = boxesresult[i];
73 VEC_COPY(contact->m_point,pout);
74 VEC_COPY(contact->m_normal,tridata.m_planes.m_planes[0]);
75
76 gim_trimesh_unlocks_work_data(trimesh);
77 GIM_DYNARRAY_DESTROY(collision_result);
78 return 1;
79 }
80 }
81
82 gim_trimesh_unlocks_work_data(trimesh);
83 GIM_DYNARRAY_DESTROY(collision_result);
84 return 0;//no collisiion
85}
86
87
88//! Trimesh Ray Collisions closest
89/*!
90Find the closest primitive collided by the ray
91\param trimesh
92\param contact
93\return 1 if the ray collides, else 0
94*/
95int gim_trimesh_ray_closest_collision(GIM_TRIMESH * trimesh,vec3f origin,vec3f dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA * contact)
96{
97 GDYNAMIC_ARRAY collision_result;
98 GIM_CREATE_BOXQUERY_LIST(collision_result);
99
100 gim_aabbset_ray_collision(origin,dir,tmax,&trimesh->m_aabbset,&collision_result);
101
102 if(collision_result.m_size==0)
103 {
104 GIM_DYNARRAY_DESTROY(collision_result);
105 return 0;
106 }
107
108 //collide triangles
109
110 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
111 GIM_TRIANGLE_DATA tridata;
112 vec3f pout;
113 GREAL tparam,u,v;
114 char does_intersect;
115 contact->tparam = tmax + 0.1f;
116
117
118 gim_trimesh_locks_work_data(trimesh);
119
120 for(unsigned int i=0;i<collision_result.m_size;i++)
121 {
122 gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tridata);
123
124 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);
125 if(does_intersect && (tparam < contact->tparam))
126 {
127 contact->tparam = tparam;
128 contact->u = u;
129 contact->v = v;
130 contact->m_face_id = boxesresult[i];
131 VEC_COPY(contact->m_point,pout);
132 VEC_COPY(contact->m_normal,tridata.m_planes.m_planes[0]);
133 }
134 }
135
136 gim_trimesh_unlocks_work_data(trimesh);
137 GIM_DYNARRAY_DESTROY(collision_result);
138 if(contact->tparam > tmax) return 0;
139 return 1;
140}
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp
new file mode 100644
index 0000000..60444fb
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp
@@ -0,0 +1,196 @@
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
31int gim_triangle_sphere_collision(
32 GIM_TRIANGLE_DATA *tri,
33 vec3f center, GREAL radius,
34 GIM_TRIANGLE_CONTACT_DATA * contact_data)
35{
36 contact_data->m_point_count = 0;
37
38 //Find Face plane distance
39 GREAL dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[0],center);
40 if(dis>radius) return 0; //out
41 if(dis<-radius) return 0;//Out of triangle
42 contact_data->m_penetration_depth = dis;
43
44 //Find the most edge
45 GUINT most_edge = 4;//no edge
46 GREAL max_dis = 0.0f;
47 dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[1],center);
48 if(dis>radius) return 0;//Out of triangle
49 if(dis>0.0f)
50 {
51 max_dis = dis;
52 most_edge = 0;
53 }
54
55 dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[2],center);
56 if(dis>radius) return 0;//Out of triangle
57 if(dis>max_dis)// && dis>0.0f)
58 {
59 max_dis = dis;
60 most_edge = 1;
61 }
62
63 dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[3],center);
64 if(dis>radius) return 0;//Out of triangle
65 if(dis>max_dis)// && dis>0.0f)
66 {
67 max_dis = dis;
68 most_edge = 2;
69 }
70
71 if(most_edge == 4) //Box is into triangle
72 {
73 //contact_data->m_penetration_depth = dis is set above
74 //Find Face plane point
75 VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[0]);
76 //Find point projection on plane
77 if(contact_data->m_penetration_depth>=0.0f)
78 {
79 VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
80 }
81 else
82 {
83 VEC_SCALE(contact_data->m_points[0],radius,contact_data->m_separating_normal);
84 }
85 contact_data->m_penetration_depth = radius - contact_data->m_penetration_depth;
86
87 VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
88 //Scale normal for pointing to triangle
89 VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
90 contact_data->m_point_count = 1;
91 return 1;
92 }
93 //find the edge
94 vec3f e1,e2;
95 VEC_COPY(e1,tri->m_vertices[most_edge]);
96 VEC_COPY(e2,tri->m_vertices[(most_edge+1)%3]);
97
98 CLOSEST_POINT_ON_SEGMENT(contact_data->m_points[0],center,e1,e2);
99 //find distance
100 VEC_DIFF(e1,center,contact_data->m_points[0]);
101 VEC_LENGTH(e1,dis);
102 if(dis>radius) return 0;
103
104 contact_data->m_penetration_depth = radius - dis;
105
106 if(IS_ZERO(dis))
107 {
108 VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[most_edge+1]);
109 VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
110 VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
111 }
112 else
113 {
114 VEC_SCALE(contact_data->m_separating_normal,1.0f/dis,e1);
115 VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
116 VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
117 }
118
119 //Scale normal for pointing to triangle
120 VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
121
122 contact_data->m_point_count = 1;
123 return 1;
124
125}
126
127//! Trimesh Sphere Collisions
128/*!
129In each contact
130<ul>
131<li> m_handle1 points to trimesh.
132<li> m_handle2 points to NULL.
133<li> m_feature1 Is a triangle index of trimesh.
134</ul>
135
136\param trimesh
137\param center
138\param radius
139\param contacts A GIM_CONTACT array. Must be initialized
140*/
141void gim_trimesh_sphere_collision(GIM_TRIMESH * trimesh,vec3f center,GREAL radius, GDYNAMIC_ARRAY * contacts)
142{
143 contacts->m_size = 0;
144
145 aabb3f test_aabb;
146 test_aabb.minX = center[0]-radius;
147 test_aabb.maxX = center[0]+radius;
148 test_aabb.minY = center[1]-radius;
149 test_aabb.maxY = center[1]+radius;
150 test_aabb.minZ = center[2]-radius;
151 test_aabb.maxZ = center[2]+radius;
152
153 GDYNAMIC_ARRAY collision_result;
154 GIM_CREATE_BOXQUERY_LIST(collision_result);
155
156 gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
157
158 if(collision_result.m_size==0)
159 {
160 GIM_DYNARRAY_DESTROY(collision_result);
161 }
162
163 //collide triangles
164 //Locks trimesh
165 gim_trimesh_locks_work_data(trimesh);
166 //dummy contacts
167 GDYNAMIC_ARRAY dummycontacts;
168 GIM_CREATE_CONTACT_LIST(dummycontacts);
169
170 int cresult;
171 unsigned int i;
172 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
173 GIM_TRIANGLE_CONTACT_DATA tri_contact_data;
174 GIM_TRIANGLE_DATA tri_data;
175
176 for(i=0;i<collision_result.m_size;i++)
177 {
178 gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
179 cresult = gim_triangle_sphere_collision(&tri_data,center,radius,&tri_contact_data);
180 if(cresult!=0)
181 {
182 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);
183 }
184 }
185 ///unlocks
186 gim_trimesh_unlocks_work_data(trimesh);
187 ///Destroy box result
188 GIM_DYNARRAY_DESTROY(collision_result);
189
190 //merge contacts
191 gim_merge_contacts(&dummycontacts,contacts);
192
193 //Destroy dummy
194 GIM_DYNARRAY_DESTROY(dummycontacts);
195}
196
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_trimesh_trimesh_collision.cpp b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_trimesh_collision.cpp
new file mode 100644
index 0000000..c9c5305
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_trimesh_trimesh_collision.cpp
@@ -0,0 +1,348 @@
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#define CLASSIFY_TRI_BY_FACE(v1,v2,v3,faceplane,out_of_face)\
32{ \
33 _distances[0] = DISTANCE_PLANE_POINT(faceplane,v1);\
34 _distances[1] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v2);\
35 _distances[2] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v3); \
36 if(_distances[1]>0.0f && _distances[2]>0.0f)\
37 {\
38 out_of_face = 1;\
39 }\
40 else\
41 {\
42 out_of_face = 0;\
43 }\
44}\
45
46
47//! Receives the 3 edge planes
48#define MOST_DEEP_POINTS(plane,points,point_count,deep_points,deep_points_count,maxdeep)\
49{\
50 maxdeep=-1000.0f;\
51 GUINT _k;\
52 GREAL _dist;\
53 deep_points_count = 0;\
54 for(_k=0;_k<point_count;_k++)\
55 {\
56 _dist = -DISTANCE_PLANE_POINT(plane,points[_k]);\
57 if(_dist>maxdeep)\
58 {\
59 maxdeep = _dist;\
60 _max_candidates[0] = _k;\
61 deep_points_count=1;\
62 }\
63 else if((_dist+G_EPSILON)>=maxdeep)\
64 {\
65 _max_candidates[deep_points_count] = _k;\
66 deep_points_count++;\
67 }\
68 }\
69 if(maxdeep<0.0f)\
70 {\
71 deep_points_count = 0;\
72 }\
73 else\
74 {\
75 for(_k=0;_k<deep_points_count;_k++)\
76 {\
77 VEC_COPY(deep_points[_k],points[_max_candidates[_k]]);\
78 }\
79 }\
80}\
81
82//! Receives the 3 edge planes
83#define CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri_points,tri_edge_planes, clipped_points, clipped_point_count)\
84{\
85 clipped_point_count = 0; \
86 _temp_clip_count = 0;\
87 PLANE_CLIP_POLYGON(tri_edge_planes[0],tri_points,3,_temp_clip,_temp_clip_count,MAX_TRI_CLIPPING);\
88 if(_temp_clip_count>0)\
89 {\
90 _temp_clip_count2 = 0;\
91 PLANE_CLIP_POLYGON(tri_edge_planes[1],_temp_clip,_temp_clip_count,_temp_clip2,_temp_clip_count2,MAX_TRI_CLIPPING);\
92 if(_temp_clip_count2>0)\
93 {\
94 PLANE_CLIP_POLYGON(tri_edge_planes[2],_temp_clip2,_temp_clip_count2,clipped_points,clipped_point_count,MAX_TRI_CLIPPING);\
95 }\
96 }\
97}\
98
99
100
101int _gim_triangle_triangle_collision(
102 GIM_TRIANGLE_DATA *tri1,
103 GIM_TRIANGLE_DATA *tri2,
104 GIM_TRIANGLE_CONTACT_DATA * contact_data)
105{
106 //Cache variables for triangle intersection
107 GUINT _max_candidates[MAX_TRI_CLIPPING];
108 vec3f _temp_clip[MAX_TRI_CLIPPING];
109 GUINT _temp_clip_count = 0;
110 vec3f _temp_clip2[MAX_TRI_CLIPPING];
111 GUINT _temp_clip_count2 = 0;
112 vec3f clipped_points2[MAX_TRI_CLIPPING];
113 vec3f deep_points2[MAX_TRI_CLIPPING];
114 vec3f clipped_points1[MAX_TRI_CLIPPING];
115 vec3f deep_points1[MAX_TRI_CLIPPING];
116
117
118
119 //State variabnles
120 GUINT mostdir=0;
121 GUINT clipped2_count=0;
122
123 //Clip tri2 by tri1 edges
124
125 CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri2->m_vertices,(&tri1->m_planes.m_planes[1]), clipped_points2, clipped2_count);
126
127 if(clipped2_count == 0 )
128 {
129 return 0;//Reject
130 }
131
132 //find most deep interval face1
133 GUINT deep2_count=0;
134
135 GREAL maxdeep;
136
137 MOST_DEEP_POINTS((tri1->m_planes.m_planes[0]), clipped_points2, clipped2_count, deep_points2, deep2_count, maxdeep);
138 if(deep2_count==0)
139 {
140// *perror = 0.0f;
141 return 0;//Reject
142 }
143
144 //Normal pointing to triangle1
145 VEC_SCALE(contact_data->m_separating_normal,-1.0f,(tri1->m_planes.m_planes[0]));
146
147
148 //Clip tri1 by tri2 edges
149
150 GUINT clipped1_count=0;
151
152 CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri1->m_vertices,(&tri2->m_planes.m_planes[1]), clipped_points1, clipped1_count);
153
154 if(clipped2_count == 0 )
155 {
156// *perror = 0.0f;
157 return 0;//Reject
158 }
159
160
161 //find interval face2
162 GUINT deep1_count=0;
163
164 GREAL dist;
165
166 MOST_DEEP_POINTS((tri2->m_planes.m_planes[0]), clipped_points1, clipped1_count, deep_points1, deep1_count, dist);
167
168 if(deep1_count==0)
169 {
170// *perror = 0.0f;
171 return 0;
172 }
173
174 if(dist<maxdeep)
175 {
176 maxdeep = dist;
177 mostdir = 1;
178 VEC_COPY(contact_data->m_separating_normal,(tri2->m_planes.m_planes[0]));
179 }
180 //set deep
181 contact_data->m_penetration_depth = maxdeep;
182
183 ////check most dir for contacts
184 if(mostdir==0)
185 {
186 contact_data->m_point_count = deep2_count;
187 for(mostdir=0;mostdir<deep2_count;mostdir++)
188 {
189 VEC_COPY(contact_data->m_points[mostdir] ,deep_points2[mostdir]);
190 }
191 }
192 else
193 {
194 contact_data->m_point_count = deep1_count;
195 for(mostdir=0;mostdir<deep1_count;mostdir++)
196 {
197 VEC_COPY(contact_data->m_points[mostdir] ,deep_points1[mostdir]);
198 }
199 }
200 return 1;
201}
202
203
204
205//! Finds the contact points from a collision of two triangles
206/*!
207Returns the contact points, the penetration depth and the separating normal of the collision
208between two triangles. The normal is pointing toward triangle 1 from triangle 2
209*/
210int gim_triangle_triangle_collision(
211 GIM_TRIANGLE_DATA *tri1,
212 GIM_TRIANGLE_DATA *tri2,
213 GIM_TRIANGLE_CONTACT_DATA * contact_data)
214{
215 vec3f _distances;
216 char out_of_face=0;
217
218 CLASSIFY_TRI_BY_FACE(tri1->m_vertices[0],tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_planes.m_planes[0],out_of_face);
219 if(out_of_face==1) return 0;
220
221 CLASSIFY_TRI_BY_FACE(tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2],tri1->m_planes.m_planes[0],out_of_face);
222 if(out_of_face==1) return 0;
223
224 return _gim_triangle_triangle_collision(tri1,tri2,contact_data);
225}
226
227//! Trimesh Trimesh Collisions
228/*!
229
230In each contact
231<ul>
232<li> m_handle1 points to trimesh1.
233<li> m_handle2 points to trimesh2.
234<li> m_feature1 Is a triangle index of trimesh1.
235<li> m_feature2 Is a triangle index of trimesh2.
236</ul>
237
238\param trimesh1 Collider
239\param trimesh2 Collidee
240\param contacts A GIM_CONTACT array. Must be initialized
241*/
242void gim_trimesh_trimesh_collision(GIM_TRIMESH * trimesh1, GIM_TRIMESH * trimesh2, GDYNAMIC_ARRAY * contacts)
243{
244 contacts->m_size = 0;
245 GDYNAMIC_ARRAY collision_pairs;
246 GIM_CREATE_PAIR_SET(collision_pairs)
247
248 gim_aabbset_bipartite_intersections(&trimesh1->m_aabbset,&trimesh2->m_aabbset,&collision_pairs);
249
250 if(collision_pairs.m_size==0)
251 {
252 GIM_DYNARRAY_DESTROY(collision_pairs);
253 return; //no collisioin
254 }
255
256 //Locks meshes
257 gim_trimesh_locks_work_data(trimesh1);
258 gim_trimesh_locks_work_data(trimesh2);
259
260
261 //pair pointer
262 GIM_PAIR *pairs = GIM_DYNARRAY_POINTER(GIM_PAIR,collision_pairs);
263 //dummy contacts
264 GDYNAMIC_ARRAY dummycontacts;
265 GIM_CREATE_CONTACT_LIST(dummycontacts);
266
267 //Auxiliary triangle data
268 GIM_TRIANGLE_CONTACT_DATA tri_contact_data;
269 GIM_TRIANGLE_DATA tri1data,tri2data;
270
271
272 GUINT i, ti1,ti2,ci;
273 int colresult;
274 for (i=0;i<collision_pairs.m_size; i++)
275 {
276 ti1 = pairs[i].m_index1;
277 ti2 = pairs[i].m_index2;
278 //Get triangles data
279 gim_trimesh_get_triangle_data(trimesh1,ti1,&tri1data);
280 gim_trimesh_get_triangle_data(trimesh2,ti2,&tri2data);
281
282 //collide triangles
283 colresult = gim_triangle_triangle_collision(&tri1data,&tri2data,&tri_contact_data);
284 if(colresult == 1)
285 {
286 //Add contacts
287 for (ci=0;ci<tri_contact_data.m_point_count ;ci++ )
288 {
289 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);
290 }
291 }
292 }
293
294 if(dummycontacts.m_size == 0) //reject
295 {
296 GIM_DYNARRAY_DESTROY(dummycontacts);
297 GIM_DYNARRAY_DESTROY(collision_pairs);
298 return;
299 }
300 //merge contacts
301 gim_merge_contacts(&dummycontacts,contacts);
302
303 //Terminate
304 GIM_DYNARRAY_DESTROY(dummycontacts);
305 GIM_DYNARRAY_DESTROY(collision_pairs);
306
307 //Unlocks meshes
308 gim_trimesh_unlocks_work_data(trimesh1);
309 gim_trimesh_unlocks_work_data(trimesh2);
310}
311
312
313//! Trimesh Plane Collisions
314/*!
315
316\param trimesh
317\param plane vec4f plane
318\param contacts A vec4f array. Must be initialized (~100). Each element have the coordinate point in the first 3 elements, and vec4f[3] has the penetration depth.
319*/
320void gim_trimesh_plane_collision(GIM_TRIMESH * trimesh,vec4f plane, GDYNAMIC_ARRAY * contacts)
321{
322 contacts->m_size = 0;
323 char classify;
324 PLANE_CLASSIFY_BOX(plane,trimesh->m_aabbset.m_global_bound,classify);
325 if(classify>1) return; // in front of plane
326
327 //Locks mesh
328 gim_trimesh_locks_work_data(trimesh);
329 //Get vertices
330 GUINT i, vertcount = trimesh->m_transformed_vertex_buffer.m_element_count;
331 vec3f * vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
332
333 GREAL dist;
334 vec4f * result_contact;
335
336 for (i=0;i<vertcount;i++)
337 {
338 dist = DISTANCE_PLANE_POINT(plane,vertices[i]);
339 if(dist<=0.0f)
340 {
341 GIM_DYNARRAY_PUSH_EMPTY(vec4f,(*contacts));
342 result_contact = GIM_DYNARRAY_POINTER_LAST(vec4f,(*contacts));
343 VEC_COPY((*result_contact),vertices[i]);
344 (*result_contact)[3] = -dist;
345 }
346 }
347 gim_trimesh_unlocks_work_data(trimesh);
348}
diff --git a/libraries/ode-0.9/GIMPACT/src/gimpact.cpp b/libraries/ode-0.9/GIMPACT/src/gimpact.cpp
new file mode 100644
index 0000000..18f1fc8
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gimpact.cpp
@@ -0,0 +1,39 @@
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/gimpact.h"
30
31void gimpact_init()
32{
33 gim_init_math();
34 gim_init_buffer_managers();
35}
36void gimpact_terminate()
37{
38 gim_terminate_buffer_managers();
39}
generated by cgit v1.1 at 2024-11-19 03:49:45 +0000