diff options
author | dan miller | 2007-10-19 05:22:23 +0000 |
---|---|---|
committer | dan miller | 2007-10-19 05:22:23 +0000 |
commit | 1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch) | |
tree | 51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp | |
parent | one more for the gipper (diff) | |
download | opensim-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.cpp | 218 |
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 | |||
30 | int dGeomTransformGroupClass = -1; | ||
31 | // ############################################################################ | ||
32 | |||
33 | struct dxGeomTransformGroup { | ||
34 | dArray<dxGeom*> parts; // all the geoms that make up the group | ||
35 | dVector3 relativePosition; | ||
36 | dMatrix3 relativeRotation; | ||
37 | }; | ||
38 | // ############################################################################ | ||
39 | |||
40 | void 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 | |||
50 | void 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 | |||
58 | const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g) | ||
59 | { | ||
60 | dAASSERT (g); | ||
61 | dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); | ||
62 | return transformGroup->relativePosition; | ||
63 | } | ||
64 | // ############################################################################ | ||
65 | |||
66 | const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g) | ||
67 | { | ||
68 | dAASSERT (g); | ||
69 | dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); | ||
70 | return transformGroup->relativeRotation; | ||
71 | } | ||
72 | // ############################################################################ | ||
73 | |||
74 | static 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 | |||
85 | int 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 | |||
121 | static dColliderFn * dGeomTransformGroupColliderFn (int num) | ||
122 | { | ||
123 | return (dColliderFn *) &dCollideTransformGroup; | ||
124 | } | ||
125 | // ############################################################################ | ||
126 | |||
127 | static 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 | |||
148 | static void dGeomTransformGroupDtor (dxGeom *geom) | ||
149 | { | ||
150 | dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom); | ||
151 | transformGroup->parts.~dArray(); | ||
152 | } | ||
153 | // ############################################################################ | ||
154 | |||
155 | dxGeom *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 | |||
179 | void 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 | |||
188 | void 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 | |||
202 | dxGeom * 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 | |||
212 | int 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 | } | ||