aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp')
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp196
1 files changed, 196 insertions, 0 deletions
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
generated by cgit v1.1 at 2024-10-15 17:26:09 +0000