aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:24:38 +0000
committerdan miller2007-10-19 05:24:38 +0000
commitf205de7847da7ae1c10212d82e7042d0100b4ce0 (patch)
tree9acc9608a6880502aaeda43af52c33e278e95b9c /libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
parenttrying to fix my screwup part deux (diff)
downloadopensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.zip
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.gz
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.bz2
opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.xz
from the start... checking in ode-0.9
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp213
1 files changed, 213 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
new file mode 100644
index 0000000..54034aa
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
@@ -0,0 +1,213 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh - Plane collider by David Walters, July 2006
24
25#include <ode/collision.h>
26#include <ode/matrix.h>
27#include <ode/rotation.h>
28#include <ode/odemath.h>
29
30#if dTRIMESH_ENABLED
31
32#include "collision_util.h"
33#include "collision_std.h"
34
35#define TRIMESH_INTERNAL
36#include "collision_trimesh_internal.h"
37
38#if dTRIMESH_OPCODE
39int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
40{
41 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
42 dIASSERT( o1->type == dTriMeshClass );
43 dIASSERT( o2->type == dPlaneClass );
44 dIASSERT ((flags & NUMC_MASK) >= 1);
45
46 // Alias pointers to the plane and trimesh
47 dxTriMesh* trimesh = (dxTriMesh*)( o1 );
48 dxPlane* plane = (dxPlane*)( o2 );
49
50 int contact_count = 0;
51
52 // Cache the maximum contact count.
53 const int contact_max = ( flags & NUMC_MASK );
54
55 // Cache trimesh position and rotation.
56 const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
57 const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
58
59 //
60 // For all triangles.
61 //
62
63 // Cache the triangle count.
64 const int tri_count = trimesh->Data->Mesh.GetNbTriangles();
65
66 VertexPointers VP;
67 dReal alpha;
68 dVector3 vertex;
69
70#if !defined(dSINGLE) || 1
71 dVector3 int_vertex; // Intermediate vertex for double precision mode.
72#endif // dSINGLE
73
74 // For each triangle
75 for ( int t = 0; t < tri_count; ++t )
76 {
77 // Get triangle, which should also use callback.
78 trimesh->Data->Mesh.GetTriangle( VP, t );
79
80 // For each vertex.
81 for ( int v = 0; v < 3; ++v )
82 {
83 //
84 // Get Vertex
85 //
86
87#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation
88
89 dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
90
91#else // dDOUBLE || 1
92
93 // OPCODE data is in single precision format.
94 int_vertex[ 0 ] = VP.Vertex[ v ]->x;
95 int_vertex[ 1 ] = VP.Vertex[ v ]->y;
96 int_vertex[ 2 ] = VP.Vertex[ v ]->z;
97
98 dMULTIPLY0_331( vertex, trimesh_R, int_vertex );
99
100#endif // dSINGLE/dDOUBLE
101
102 vertex[ 0 ] += trimesh_pos[ 0 ];
103 vertex[ 1 ] += trimesh_pos[ 1 ];
104 vertex[ 2 ] += trimesh_pos[ 2 ];
105
106
107 //
108 // Collision?
109 //
110
111 // If alpha < 0 then point is if front of plane. i.e. no contact
112 // If alpha = 0 then the point is on the plane
113 alpha = plane->p[ 3 ] - dDOT( plane->p, vertex );
114
115 // If alpha > 0 the point is behind the plane. CONTACT!
116 if ( alpha > 0 )
117 {
118 // Alias the contact
119 dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
120
121 contact->pos[ 0 ] = vertex[ 0 ];
122 contact->pos[ 1 ] = vertex[ 1 ];
123 contact->pos[ 2 ] = vertex[ 2 ];
124
125 contact->normal[ 0 ] = plane->p[ 0 ];
126 contact->normal[ 1 ] = plane->p[ 1 ];
127 contact->normal[ 2 ] = plane->p[ 2 ];
128
129 contact->depth = alpha;
130 contact->g1 = plane;
131 contact->g2 = trimesh;
132
133 ++contact_count;
134
135 // All contact slots are full?
136 if ( contact_count >= contact_max )
137 return contact_count; // <=== STOP HERE
138 }
139 }
140 }
141
142 // Return contact count.
143 return contact_count;
144}
145#endif // dTRIMESH_OPCODE
146
147#if dTRIMESH_GIMPACT
148int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
149{
150 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
151 dIASSERT( o1->type == dTriMeshClass );
152 dIASSERT( o2->type == dPlaneClass );
153 dIASSERT ((flags & NUMC_MASK) >= 1);
154
155 // Alias pointers to the plane and trimesh
156 dxTriMesh* trimesh = (dxTriMesh*)( o1 );
157 vec4f plane;
158 dGeomPlaneGetParams(o2, plane);
159
160 //Find collision
161
162 GDYNAMIC_ARRAY collision_result;
163 GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
164
165 gim_trimesh_plane_collision(&trimesh->m_collision_trimesh,plane,&collision_result);
166
167 if(collision_result.m_size == 0 )
168 {
169 GIM_DYNARRAY_DESTROY(collision_result);
170 return 0;
171 }
172
173
174 unsigned int contactcount = collision_result.m_size;
175 unsigned int contactmax = (unsigned int)(flags & NUMC_MASK);
176 if (contactcount > contactmax)
177 {
178 contactcount = contactmax;
179 }
180
181 dContactGeom* pcontact;
182 vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
183
184 for(unsigned int i = 0; i < contactcount; i++ )
185 {
186 pcontact = SAFECONTACT(flags, contacts, i, skip);
187
188 pcontact->pos[0] = (*planecontact_results)[0];
189 pcontact->pos[1] = (*planecontact_results)[1];
190 pcontact->pos[2] = (*planecontact_results)[2];
191 pcontact->pos[3] = REAL(1.0);
192
193 pcontact->normal[0] = plane[0];
194 pcontact->normal[1] = plane[1];
195 pcontact->normal[2] = plane[2];
196 pcontact->normal[3] = 0;
197
198 pcontact->depth = (*planecontact_results)[3];
199 pcontact->g1 = o1;
200 pcontact->g2 = o2;
201
202 planecontact_results++;
203 }
204
205 GIM_DYNARRAY_DESTROY(collision_result);
206
207 return (int)contactcount;
208}
209#endif // dTRIMESH_GIMPACT
210
211
212#endif // dTRIMESH_ENABLED
213
generated by cgit v1.1 at 2024-12-26 10:10:23 +0000