Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago

[Physik] add ode-0.9
File size: 6.3 KB
Line 
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
Note: See TracBrowser for help on using the repository browser.