aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:22:23 +0000
committerdan miller2007-10-19 05:22:23 +0000
commit1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
parentone more for the gipper (diff)
downloadopensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.zip
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.gz
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.bz2
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.xz
trying to fix my screwup, please hold on
Diffstat (limited to 'libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp')
-rw-r--r--libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp218
1 files changed, 0 insertions, 218 deletions
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
deleted file mode 100644
index 26b77b0..0000000
--- a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
1
2/* ************************************************************************ */
3/*
4 grouped and transformed geometry functions
5 author: Tim Schmidt tisch@uni-paderborn.de
6*/
7
8
9#include <ode/common.h>
10#include <ode/geom.h>
11#include <ode/rotation.h>
12#include <ode/odemath.h>
13#include <ode/memory.h>
14#include <ode/misc.h>
15#include <ode/objects.h>
16#include <ode/matrix.h>
17#include <ode/GeomTransformGroup.h>
18#include "objects.h"
19#include "array.h"
20#include "geom_internal.h"
21
22// given a pointer `p' to a dContactGeom, return the dContactGeom at
23// p + skip bytes.
24
25#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
26
27
28// ############################################################################
29
30int dGeomTransformGroupClass = -1;
31// ############################################################################
32
33struct dxGeomTransformGroup {
34 dArray<dxGeom*> parts; // all the geoms that make up the group
35 dVector3 relativePosition;
36 dMatrix3 relativeRotation;
37};
38// ############################################################################
39
40void dGeomTransformGroupSetRelativePosition (dxGeom *g, dReal x, dReal y, dReal z)
41{
42 dAASSERT (g);
43 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
44 transformGroup->relativePosition[0] = x;
45 transformGroup->relativePosition[1] = y;
46 transformGroup->relativePosition[2] = z;
47}
48// ############################################################################
49
50void dGeomTransformGroupSetRelativeRotation (dxGeom *g, const dMatrix3 R)
51{
52 dAASSERT (g);
53 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
54 memcpy (transformGroup->relativeRotation,R,sizeof(dMatrix3));
55}
56// ############################################################################
57
58const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g)
59{
60 dAASSERT (g);
61 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
62 return transformGroup->relativePosition;
63}
64// ############################################################################
65
66const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g)
67{
68 dAASSERT (g);
69 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
70 return transformGroup->relativeRotation;
71}
72// ############################################################################
73
74static void computeFinalTransformation (const dxGeom *tg, const dxGeom *part)
75{
76 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(tg);
77 dMULTIPLY0_331 (part->pos,tg->R,transformGroup->relativePosition);
78 part->pos[0] += tg->pos[0];
79 part->pos[1] += tg->pos[1];
80 part->pos[2] += tg->pos[2];
81 dMULTIPLY0_333 (part->R,tg->R,transformGroup->relativeRotation);
82}
83// ############################################################################
84
85int dCollideTransformGroup (const dxGeom *o1, const dxGeom *o2, int flags,
86 dContactGeom *contact, int skip)
87{
88 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(o1);
89 if (transformGroup->parts.size() == 0)
90 {
91 return 0;
92 }
93 int numleft = flags & NUMC_MASK;
94 if (numleft == 0) numleft = 1;
95 flags &= ~NUMC_MASK;
96 int num=0, i=0;
97 while (i < transformGroup->parts.size() && numleft > 0)
98 {
99 dUASSERT (transformGroup->parts[i]->spaceid==0,
100 "GeomTransformGroup encapsulated object must not be in a space");
101 dUASSERT (transformGroup->parts[i]->body==0,
102 "GeomTransformGroup encapsulated object must not be attached to a body");
103 if (!o1->space_aabb)
104 {
105 computeFinalTransformation (o1, transformGroup->parts[i]);
106 }
107 dxBody *bodyBackup = transformGroup->parts[i]->body;
108 transformGroup->parts[i]->body = o1->body;
109 int n = dCollide (transformGroup->parts[i],const_cast<dxGeom*>(o2),
110 flags | numleft,contact,skip);
111 transformGroup->parts[i]->body = bodyBackup;
112 contact = CONTACT (contact,skip*n);
113 numleft -= n;
114 num += n;
115 i++;
116 }
117 return num;
118}
119// ############################################################################
120
121static dColliderFn * dGeomTransformGroupColliderFn (int num)
122{
123 return (dColliderFn *) &dCollideTransformGroup;
124}
125// ############################################################################
126
127static void dGeomTransformGroupAABB (dxGeom *geom, dReal aabb[6])
128{
129 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
130 aabb[0] = dInfinity;
131 aabb[1] = -dInfinity;
132 aabb[2] = dInfinity;
133 aabb[3] = -dInfinity;
134 aabb[4] = dInfinity;
135 aabb[5] = -dInfinity;
136 int i,j;
137 for (i=0; i < transformGroup->parts.size(); i++)
138 {
139 computeFinalTransformation (geom, transformGroup->parts[i]);
140 dReal aabb2[6];
141 transformGroup->parts[i]->_class->aabb (transformGroup->parts[i],aabb2);
142 for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j];
143 for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j];
144 }
145}
146// ############################################################################
147
148static void dGeomTransformGroupDtor (dxGeom *geom)
149{
150 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
151 transformGroup->parts.~dArray();
152}
153// ############################################################################
154
155dxGeom *dCreateGeomTransformGroup (dSpaceID space)
156{
157 if (dGeomTransformGroupClass == -1) {
158 dGeomClass c;
159 c.bytes = sizeof (dxGeomTransformGroup);
160 c.collider = &dGeomTransformGroupColliderFn;
161 c.aabb = &dGeomTransformGroupAABB;
162 c.aabb_test = 0;
163 c.dtor = dGeomTransformGroupDtor;
164 dGeomTransformGroupClass = dCreateGeomClass (&c);
165 }
166 dxGeom *g = dCreateGeom (dGeomTransformGroupClass);
167 if (space)
168 {
169 dSpaceAdd (space,g);
170 }
171 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
172 transformGroup->parts.constructor();
173 dSetZero (transformGroup->relativePosition,4);
174 dRSetIdentity (transformGroup->relativeRotation);
175 return g;
176}
177// ############################################################################
178
179void dGeomTransformGroupAddGeom (dxGeom *g, dxGeom *obj)
180{
181 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
182 "argument not a geom TransformGroup");
183 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
184 transformGroup->parts.push (obj);
185}
186// ############################################################################
187
188void dGeomTransformGroupRemoveGeom (dxGeom *g, dxGeom *obj)
189{
190 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
191 "argument not a geom TransformGroup");
192 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
193 for (int i=0; i < transformGroup->parts.size(); i++) {
194 if (transformGroup->parts[i] == obj) {
195 transformGroup->parts.remove (i);
196 return;
197 }
198 }
199}
200// ############################################################################
201
202dxGeom * dGeomTransformGroupGetGeom (dxGeom *g, int i)
203{
204 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
205 "argument not a geom TransformGroup");
206 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
207 dAASSERT (i >= 0 && i < transformGroup->parts.size());
208 return transformGroup->parts[i];
209}
210// ############################################################################
211
212int dGeomTransformGroupGetNumGeoms (dxGeom *g)
213{
214 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
215 "argument not a geom TransformGroup");
216 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
217 return transformGroup->parts.size();
218}