diff options
| author | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
|---|---|---|
| committer | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
| commit | c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch) | |
| tree | 277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/GIMPACT/src/gim_trimesh_sphere_collision.cpp | |
| parent | 1cf9cc3408af7008451f9133fb95af66a9697d15 (diff) | |
add ode
Diffstat (limited to 'libs/ode-0.16.1/GIMPACT/src/gim_trimesh_sphere_collision.cpp')
| -rw-r--r-- | libs/ode-0.16.1/GIMPACT/src/gim_trimesh_sphere_collision.cpp | 196 |
1 files changed, 196 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/GIMPACT/src/gim_trimesh_sphere_collision.cpp b/libs/ode-0.16.1/GIMPACT/src/gim_trimesh_sphere_collision.cpp new file mode 100644 index 0000000..0d61715 --- /dev/null +++ b/libs/ode-0.16.1/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
+ GUINT32 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
+<ul>
+<li> m_handle1 points to trimesh.
+<li> m_handle2 points to NULL.
+<li> m_feature1 Is a triangle index of trimesh.
+</ul>
+
+\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;
+ GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
+ GIM_TRIANGLE_CONTACT_DATA tri_contact_data;
+ GIM_TRIANGLE_DATA tri_data;
+
+ for(i=0;i<collision_result.m_size;i++)
+ {
+ gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
+ cresult = gim_triangle_sphere_collision(&tri_data,center,radius,&tri_contact_data);
+ if(cresult!=0)
+ {
+ 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);
+ }
+ }
+ ///unlocks
+ gim_trimesh_unlocks_work_data(trimesh);
+ ///Destroy box result
+ GIM_DYNARRAY_DESTROY(collision_result);
+
+ //merge contacts
+ gim_merge_contacts(&dummycontacts,contacts);
+
+ //Destroy dummy
+ GIM_DYNARRAY_DESTROY(dummycontacts);
+}
+
|