From f205de7847da7ae1c10212d82e7042d0100b4ce0 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:24:38 +0000 Subject: from the start... checking in ode-0.9 --- .../GIMPACT/src/gim_trimesh_sphere_collision.cpp | 196 +++++++++++++++++++++ 1 file changed, 196 insertions(+) create mode 100644 libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp (limited to 'libraries/ode-0.9/GIMPACT/src/gim_trimesh_sphere_collision.cpp') 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 @@ + +/* +----------------------------------------------------------------------------- +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2006 Francisco Leon. C.C. 80087371. +email: projectileman@yahoo.com + + This library is free software; you can redistribute it and/or + modify it under the terms of EITHER: + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file GIMPACT-LICENSE-LGPL.TXT. + (2) The BSD-style license that is included with this library in + the file GIMPACT-LICENSE-BSD.TXT. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details. + +----------------------------------------------------------------------------- +*/ + +#include "GIMPACT/gim_trimesh.h" + +int gim_triangle_sphere_collision( + GIM_TRIANGLE_DATA *tri, + vec3f center, GREAL radius, + GIM_TRIANGLE_CONTACT_DATA * contact_data) +{ + contact_data->m_point_count = 0; + + //Find Face plane distance + GREAL dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[0],center); + if(dis>radius) return 0; //out + if(dis<-radius) return 0;//Out of triangle + contact_data->m_penetration_depth = dis; + + //Find the most edge + GUINT most_edge = 4;//no edge + GREAL max_dis = 0.0f; + dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[1],center); + if(dis>radius) return 0;//Out of triangle + if(dis>0.0f) + { + max_dis = dis; + most_edge = 0; + } + + dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[2],center); + if(dis>radius) return 0;//Out of triangle + if(dis>max_dis)// && dis>0.0f) + { + max_dis = dis; + most_edge = 1; + } + + dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[3],center); + if(dis>radius) return 0;//Out of triangle + if(dis>max_dis)// && dis>0.0f) + { + max_dis = dis; + most_edge = 2; + } + + if(most_edge == 4) //Box is into triangle + { + //contact_data->m_penetration_depth = dis is set above + //Find Face plane point + VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[0]); + //Find point projection on plane + if(contact_data->m_penetration_depth>=0.0f) + { + VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal); + } + else + { + VEC_SCALE(contact_data->m_points[0],radius,contact_data->m_separating_normal); + } + contact_data->m_penetration_depth = radius - contact_data->m_penetration_depth; + + VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center); + //Scale normal for pointing to triangle + VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal); + contact_data->m_point_count = 1; + return 1; + } + //find the edge + vec3f e1,e2; + VEC_COPY(e1,tri->m_vertices[most_edge]); + VEC_COPY(e2,tri->m_vertices[(most_edge+1)%3]); + + CLOSEST_POINT_ON_SEGMENT(contact_data->m_points[0],center,e1,e2); + //find distance + VEC_DIFF(e1,center,contact_data->m_points[0]); + VEC_LENGTH(e1,dis); + if(dis>radius) return 0; + + contact_data->m_penetration_depth = radius - dis; + + if(IS_ZERO(dis)) + { + VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[most_edge+1]); + VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal); + VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center); + } + else + { + VEC_SCALE(contact_data->m_separating_normal,1.0f/dis,e1); + VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal); + VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center); + } + + //Scale normal for pointing to triangle + VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal); + + contact_data->m_point_count = 1; + return 1; + +} + +//! Trimesh Sphere Collisions +/*! +In each contact + + +\param trimesh +\param center +\param radius +\param contacts A GIM_CONTACT array. Must be initialized +*/ +void gim_trimesh_sphere_collision(GIM_TRIMESH * trimesh,vec3f center,GREAL radius, GDYNAMIC_ARRAY * contacts) +{ + contacts->m_size = 0; + + aabb3f test_aabb; + test_aabb.minX = center[0]-radius; + test_aabb.maxX = center[0]+radius; + test_aabb.minY = center[1]-radius; + test_aabb.maxY = center[1]+radius; + test_aabb.minZ = center[2]-radius; + test_aabb.maxZ = center[2]+radius; + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result); + + if(collision_result.m_size==0) + { + GIM_DYNARRAY_DESTROY(collision_result); + } + + //collide triangles + //Locks trimesh + gim_trimesh_locks_work_data(trimesh); + //dummy contacts + GDYNAMIC_ARRAY dummycontacts; + GIM_CREATE_CONTACT_LIST(dummycontacts); + + int cresult; + unsigned int i; + GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result); + GIM_TRIANGLE_CONTACT_DATA tri_contact_data; + GIM_TRIANGLE_DATA tri_data; + + for(i=0;i