aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:48 +0000
committerdan miller2007-10-19 05:20:48 +0000
commitd48ea5bb797037069d641da41da0f195f0124491 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
parentdont ask (diff)
downloadopensim-SC-d48ea5bb797037069d641da41da0f195f0124491.zip
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.gz
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.bz2
opensim-SC-d48ea5bb797037069d641da41da0f195f0124491.tar.xz
one more for the gipper
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp456
1 files changed, 456 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
new file mode 100644
index 0000000..229966b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
@@ -0,0 +1,456 @@
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#include <ode/collision.h>
24#include <ode/matrix.h>
25#include <ode/rotation.h>
26#include <ode/odemath.h>
27
28#if dTRIMESH_ENABLED
29
30#include "collision_util.h"
31#define TRIMESH_INTERNAL
32#include "collision_trimesh_internal.h"
33
34#if dTRIMESH_GIMPACT
35
36void dxTriMeshData::Preprocess(){ // stub
37}
38
39dTriMeshDataID dGeomTriMeshDataCreate(){
40 return new dxTriMeshData();
41}
42
43void dGeomTriMeshDataDestroy(dTriMeshDataID g){
44 delete g;
45}
46
47void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) { //stub
48}
49
50dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) {
51 return NULL; // stub
52}
53
54void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) { //stub
55}
56
57void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) {
58 dUASSERT(g, "argument not trimesh data");
59 return NULL; // stub
60}
61
62void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
63 const void* Vertices, int VertexStride, int VertexCount,
64 const void* Indices, int IndexCount, int TriStride,
65 const void* Normals)
66{
67 dUASSERT(g, "argument not trimesh data");
68 dIASSERT(Vertices);
69 dIASSERT(Indices);
70
71 g->Build(Vertices, VertexStride, VertexCount,
72 Indices, IndexCount, TriStride,
73 Normals,
74 true);
75}
76
77
78void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
79 const void* Vertices, int VertexStride, int VertexCount,
80 const void* Indices, int IndexCount, int TriStride)
81{
82 dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
83 Indices, IndexCount, TriStride, (void*)NULL);
84}
85
86
87void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
88 const void* Vertices, int VertexStride, int VertexCount,
89 const void* Indices, int IndexCount, int TriStride,
90 const void* Normals)
91{
92 dUASSERT(g, "argument not trimesh data");
93
94 g->Build(Vertices, VertexStride, VertexCount,
95 Indices, IndexCount, TriStride,
96 Normals,
97 false);
98}
99
100
101void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
102 const void* Vertices, int VertexStride, int VertexCount,
103 const void* Indices, int IndexCount, int TriStride) {
104 dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
105 Indices, IndexCount, TriStride, NULL);
106}
107
108
109void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
110 const dReal* Vertices, int VertexCount,
111 const int* Indices, int IndexCount,
112 const int* Normals){
113#ifdef dSINGLE
114 dGeomTriMeshDataBuildSingle1(g,
115 Vertices, 4 * sizeof(dReal), VertexCount,
116 Indices, IndexCount, 3 * sizeof(unsigned int),
117 Normals);
118#else
119 dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
120 Indices, IndexCount, 3 * sizeof(unsigned int),
121 Normals);
122#endif
123}
124
125
126void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
127 const dReal* Vertices, int VertexCount,
128 const int* Indices, int IndexCount) {
129 dGeomTriMeshDataBuildSimple1(g,
130 Vertices, VertexCount, Indices, IndexCount,
131 (const int*)NULL);
132}
133
134void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
135{
136 dUASSERT(g, "argument not trimesh data");
137 g->Preprocess();
138}
139
140void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
141{
142 dUASSERT(g, "argument not trimesh data");
143 *buf = NULL;
144 *bufLen = 0;
145}
146
147void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
148{
149 dUASSERT(g, "argument not trimesh data");
150// g->UseFlags = buf;
151}
152
153
154// Trimesh
155
156dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){
157 type = dTriMeshClass;
158
159 dGeomTriMeshSetData(this,Data);
160
161 /* TC has speed/space 'issues' that don't make it a clear
162 win by default on spheres/boxes. */
163 this->doSphereTC = true;
164 this->doBoxTC = true;
165 this->doCapsuleTC = true;
166
167}
168
169dxTriMesh::~dxTriMesh(){
170
171 //Terminate Trimesh
172 gim_trimesh_destroy(&m_collision_trimesh);
173}
174
175
176void dxTriMesh::ClearTCCache(){
177
178}
179
180
181int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
182 return 1;
183}
184
185
186void dxTriMesh::computeAABB()
187{
188 //update trimesh transform
189 mat4f transform;
190 IDENTIFY_MATRIX_4X4(transform);
191 MakeMatrix(this, transform);
192 gim_trimesh_set_tranform(&m_collision_trimesh,transform);
193
194 //Update trimesh boxes
195 gim_trimesh_update(&m_collision_trimesh);
196
197 memcpy(aabb,&m_collision_trimesh.m_aabbset.m_global_bound,6*sizeof(GREAL));
198}
199
200
201void dxTriMeshData::UpdateData()
202{
203// BVTree.Refit();
204}
205
206
207dGeomID dCreateTriMesh(dSpaceID space,
208 dTriMeshDataID Data,
209 dTriCallback* Callback,
210 dTriArrayCallback* ArrayCallback,
211 dTriRayCallback* RayCallback)
212{
213 dxTriMesh* Geom = new dxTriMesh(space, Data);
214 Geom->Callback = Callback;
215 Geom->ArrayCallback = ArrayCallback;
216 Geom->RayCallback = RayCallback;
217
218 return Geom;
219}
220
221void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
222{
223 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
224 ((dxTriMesh*)g)->Callback = Callback;
225}
226
227dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
228{
229 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
230 return ((dxTriMesh*)g)->Callback;
231}
232
233void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
234{
235 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
236 ((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
237}
238
239dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
240{
241 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
242 return ((dxTriMesh*)g)->ArrayCallback;
243}
244
245void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
246{
247 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
248 ((dxTriMesh*)g)->RayCallback = Callback;
249}
250
251dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
252{
253 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
254 return ((dxTriMesh*)g)->RayCallback;
255}
256
257void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
258{
259 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
260 dxTriMesh* mesh = (dxTriMesh*) g;
261 mesh->Data = Data;
262 // I changed my data -- I know nothing about my own AABB anymore.
263 ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD);
264
265 // GIMPACT only supports stride 12, so we need to catch the error early.
266 dUASSERT
267 (
268 Data->m_VertexStride == 3*sizeof(dReal) && Data->m_TriStride == 3*sizeof(int),
269 "Gimpact trimesh only supports a stride of 3 dReal/int\n"
270 "This means that you cannot use dGeomTriMeshDataBuildSimple() with Gimpact.\n"
271 "Change the stride, or use Opcode trimeshes instead.\n"
272 );
273
274 //Create trimesh
275 if ( Data->m_Vertices )
276 gim_trimesh_create_from_data
277 (
278 &mesh->m_collision_trimesh, // gimpact mesh
279 ( vec3f *)(&Data->m_Vertices[0]), // vertices
280 Data->m_VertexCount, // nr of verts
281 0, // copy verts?
282 ( GUINT *)(&Data->m_Indices[0]), // indices
283 Data->m_TriangleCount*3, // nr of indices
284 0, // copy indices?
285 1 // transformed reply
286 );
287}
288
289dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
290{
291 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
292 return ((dxTriMesh*)g)->Data;
293}
294
295
296
297void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
298{
299 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
300
301 switch (geomClass)
302 {
303 case dSphereClass:
304 ((dxTriMesh*)g)->doSphereTC = (1 == enable);
305 break;
306 case dBoxClass:
307 ((dxTriMesh*)g)->doBoxTC = (1 == enable);
308 break;
309 case dCapsuleClass:
310// case dCCylinderClass:
311 ((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
312 break;
313 }
314}
315
316int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
317{
318 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
319
320 switch (geomClass)
321 {
322 case dSphereClass:
323 if (((dxTriMesh*)g)->doSphereTC)
324 return 1;
325 break;
326 case dBoxClass:
327 if (((dxTriMesh*)g)->doBoxTC)
328 return 1;
329 break;
330 case dCapsuleClass:
331 if (((dxTriMesh*)g)->doCapsuleTC)
332 return 1;
333 break;
334 }
335 return 0;
336}
337
338void dGeomTriMeshClearTCCache(dGeomID g){
339 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
340
341 dxTriMesh* Geom = (dxTriMesh*)g;
342 Geom->ClearTCCache();
343}
344
345/*
346 * returns the TriMeshDataID
347 */
348dTriMeshDataID
349dGeomTriMeshGetTriMeshDataID(dGeomID g)
350{
351 dxTriMesh* Geom = (dxTriMesh*) g;
352 return Geom->Data;
353}
354
355// Getting data
356void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2)
357{
358 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
359
360 dxTriMesh* Geom = (dxTriMesh*)g;
361 gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
362 gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, (*v0),(*v1),(*v2));
363 gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
364
365}
366
367void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
368 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
369
370 dxTriMesh* Geom = (dxTriMesh*)g;
371 dVector3 dv[3];
372 gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
373 gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]);
374 GetPointFromBarycentric(dv, u, v, Out);
375 gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
376}
377
378int dGeomTriMeshGetTriangleCount (dGeomID g)
379{
380 dxTriMesh* Geom = (dxTriMesh*)g;
381 return gim_trimesh_get_triangle_count(&Geom->m_collision_trimesh);
382}
383
384void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
385 dUASSERT(g, "argument not trimesh data");
386 g->UpdateData();
387}
388
389
390//
391// GIMPACT TRIMESH-TRIMESH COLLIDER
392//
393
394int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
395{
396 dIASSERT (Stride >= (int)sizeof(dContactGeom));
397 dIASSERT (g1->type == dTriMeshClass);
398 dIASSERT (g2->type == dTriMeshClass);
399 dIASSERT ((Flags & NUMC_MASK) >= 1);
400
401 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
402 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
403 //Create contact list
404 GDYNAMIC_ARRAY trimeshcontacts;
405 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
406
407 //Collide trimeshes
408 gim_trimesh_trimesh_collision(&TriMesh1->m_collision_trimesh,&TriMesh2->m_collision_trimesh,&trimeshcontacts);
409
410 if(trimeshcontacts.m_size == 0)
411 {
412 GIM_DYNARRAY_DESTROY(trimeshcontacts);
413 return 0;
414 }
415
416 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
417
418
419 unsigned contactcount = trimeshcontacts.m_size;
420 unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
421 if (contactcount > maxcontacts)
422 {
423 contactcount = maxcontacts;
424 }
425
426 dContactGeom* pcontact;
427 unsigned i;
428
429 for (i=0;i<contactcount;i++)
430 {
431 pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
432
433 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
434 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
435 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
436 pcontact->pos[3] = 1.0f;
437
438 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
439 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
440 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
441 pcontact->normal[3] = 0;
442
443 pcontact->depth = ptrimeshcontacts->m_depth;
444 pcontact->g1 = g1;
445 pcontact->g2 = g2;
446
447 ptrimeshcontacts++;
448 }
449
450 GIM_DYNARRAY_DESTROY(trimeshcontacts);
451
452 return (int)contactcount;
453}
454
455#endif // dTRIMESH_GIMPACT
456#endif // dTRIMESH_ENABLED
generated by cgit v1.1 at 2024-12-28 08:45:34 +0000