diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp | 226 |
1 files changed, 226 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp new file mode 100644 index 0000000..5c3c67a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_plane.cpp @@ -0,0 +1,226 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * 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 LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file 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 * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh - Plane collider by David Walters, July 2006 + +#include <ode/collision.h> +#include <ode/rotation.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_std.h" +#include "collision_trimesh_internal.h" + + +#if dTRIMESH_OPCODE + +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + dxPlane* plane = (dxPlane*)( o2 ); + + int contact_count = 0; + + // Cache the maximum contact count. + const int contact_max = ( flags & NUMC_MASK ); + + // Cache trimesh position and rotation. + const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh ); + const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh ); + + // + // For all triangles. + // + + VertexPointersEx VPE; + VertexPointers &VP = VPE.vp; + ConversionArea VC; + dReal alpha; + dVector3 vertex; + +#if !defined(dSINGLE) || 1 + dVector3 int_vertex; // Intermediate vertex for double precision mode. +#endif // dSINGLE + + const unsigned uiTLSKind = trimesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == plane->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + VertexUseCache &vertex_use_cache = pccColliderCache->m_VertexUses; + + // Reallocate vertex use cache if necessary + const dxTriMeshData *meshData = trimesh->retrieveMeshData(); + const int vertex_count = meshData->m_Mesh.GetNbVertices(); + const bool cache_status = vertex_use_cache.resizeAndResetVertexUSEDFlags(vertex_count); + + // Cache the triangle count. + const int tri_count = meshData->m_Mesh.GetNbTriangles(); + + // For each triangle + for ( int t = 0; t < tri_count; ++t ) + { + // Get triangle, which should also use callback. + bool ex_avail = meshData->m_Mesh.GetExTriangle( VPE, t, VC); + + // For each vertex. + for ( int v = 0; v < 3; ++v ) + { + // point already used ? + if (cache_status && ex_avail) + { + unsigned VIndex = VPE.Index[v]; + if (vertex_use_cache.getVertexUSEDFlag(VIndex)) + continue; + // mark this point as used + vertex_use_cache.setVertexUSEDFlag(VIndex); + } + + // + // Get Vertex + // + +#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation + + dMultiply0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) ); + +#else // dDOUBLE || 1 + + // OPCODE data is in single precision format. + int_vertex[ 0 ] = VP.Vertex[ v ]->x; + int_vertex[ 1 ] = VP.Vertex[ v ]->y; + int_vertex[ 2 ] = VP.Vertex[ v ]->z; + + dMultiply0_331( vertex, trimesh_R, int_vertex ); + +#endif // dSINGLE/dDOUBLE + + vertex[ 0 ] += trimesh_pos[ 0 ]; + vertex[ 1 ] += trimesh_pos[ 1 ]; + vertex[ 2 ] += trimesh_pos[ 2 ]; + + + // + // Collision? + // + + // If alpha < 0 then point is if front of plane. i.e. no contact + // If alpha = 0 then the point is on the plane + alpha = plane->p[ 3 ] - dCalcVectorDot3( plane->p, vertex ); + + // If alpha > 0 the point is behind the plane. CONTACT! + if ( alpha > 0 ) + { + // Alias the contact + dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip ); + + contact->pos[ 0 ] = vertex[ 0 ]; + contact->pos[ 1 ] = vertex[ 1 ]; + contact->pos[ 2 ] = vertex[ 2 ]; + + contact->normal[ 0 ] = plane->p[ 0 ]; + contact->normal[ 1 ] = plane->p[ 1 ]; + contact->normal[ 2 ] = plane->p[ 2 ]; + + contact->depth = alpha; + contact->g1 = trimesh; + contact->g2 = plane; + contact->side1 = t; + contact->side2 = -1; + + ++contact_count; + + // All contact slots are full? + if ( contact_count >= contact_max ) + return contact_count; // <=== STOP HERE + } + } + } + + // Return contact count. + return contact_count; +} + + +#endif // dTRIMESH_OPCODE + + +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_plane_contact_accessor.h" + + +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + dVector4 plane; + dGeomPlaneGetParams(o2, plane); + + o1 -> recomputeAABB(); + o2 -> recomputeAABB(); + + //Find collision + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result); + + gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result); + + if(collision_result.m_size == 0 ) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } + + + vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f, collision_result); + unsigned int contactcount = collision_result.m_size; + + dxPlaneContactAccessor contactaccessor(planecontact_results, plane, o1, o2); + contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contacts, skip); + + GIM_DYNARRAY_DESTROY(collision_result); + + return (int)contactcount; +} + + +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED + |