1 | /************************************************************************* |
---|
2 | * * |
---|
3 | * Open Dynamics Engine, Copyright (C) 2001,2002 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 | /* |
---|
24 | |
---|
25 | geom transform |
---|
26 | |
---|
27 | */ |
---|
28 | |
---|
29 | #include <ode/collision.h> |
---|
30 | #include <ode/matrix.h> |
---|
31 | #include <ode/rotation.h> |
---|
32 | #include <ode/odemath.h> |
---|
33 | #include "collision_transform.h" |
---|
34 | #include "collision_util.h" |
---|
35 | |
---|
36 | #ifdef _MSC_VER |
---|
37 | #pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" |
---|
38 | #endif |
---|
39 | |
---|
40 | //**************************************************************************** |
---|
41 | // dxGeomTransform class |
---|
42 | |
---|
43 | struct dxGeomTransform : public dxGeom { |
---|
44 | dxGeom *obj; // object that is being transformed |
---|
45 | int cleanup; // 1 to destroy obj when destroyed |
---|
46 | int infomode; // 1 to put Tx geom in dContactGeom g1 |
---|
47 | |
---|
48 | // cached final object transform (body tx + relative tx). this is set by |
---|
49 | // computeAABB(), and it is valid while the AABB is valid. |
---|
50 | dxPosR transform_posr; |
---|
51 | |
---|
52 | dxGeomTransform (dSpaceID space); |
---|
53 | ~dxGeomTransform(); |
---|
54 | void computeAABB(); |
---|
55 | void computeFinalTx(); |
---|
56 | }; |
---|
57 | /* |
---|
58 | void RunMe() |
---|
59 | { |
---|
60 | printf("sizeof body = %i\n", sizeof(dxBody)); |
---|
61 | printf("sizeof geom = %i\n", sizeof(dxGeom)); |
---|
62 | printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform)); |
---|
63 | printf("sizeof posr = %i\n", sizeof(dxPosR)); |
---|
64 | } |
---|
65 | */ |
---|
66 | |
---|
67 | dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1) |
---|
68 | { |
---|
69 | type = dGeomTransformClass; |
---|
70 | obj = 0; |
---|
71 | cleanup = 0; |
---|
72 | infomode = 0; |
---|
73 | dSetZero (transform_posr.pos,4); |
---|
74 | dRSetIdentity (transform_posr.R); |
---|
75 | } |
---|
76 | |
---|
77 | |
---|
78 | dxGeomTransform::~dxGeomTransform() |
---|
79 | { |
---|
80 | if (obj && cleanup) delete obj; |
---|
81 | } |
---|
82 | |
---|
83 | |
---|
84 | void dxGeomTransform::computeAABB() |
---|
85 | { |
---|
86 | if (!obj) { |
---|
87 | dSetZero (aabb,6); |
---|
88 | return; |
---|
89 | } |
---|
90 | |
---|
91 | // backup the relative pos and R pointers of the encapsulated geom object |
---|
92 | dxPosR* posr_bak = obj->final_posr; |
---|
93 | |
---|
94 | // compute temporary pos and R for the encapsulated geom object |
---|
95 | computeFinalTx(); |
---|
96 | obj->final_posr = &transform_posr; |
---|
97 | |
---|
98 | // compute the AABB |
---|
99 | obj->computeAABB(); |
---|
100 | memcpy (aabb,obj->aabb,6*sizeof(dReal)); |
---|
101 | |
---|
102 | // restore the pos and R |
---|
103 | obj->final_posr = posr_bak; |
---|
104 | } |
---|
105 | |
---|
106 | |
---|
107 | // utility function for dCollideTransform() : compute final pos and R |
---|
108 | // for the encapsulated geom object |
---|
109 | |
---|
110 | void dxGeomTransform::computeFinalTx() |
---|
111 | { |
---|
112 | dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos); |
---|
113 | transform_posr.pos[0] += final_posr->pos[0]; |
---|
114 | transform_posr.pos[1] += final_posr->pos[1]; |
---|
115 | transform_posr.pos[2] += final_posr->pos[2]; |
---|
116 | dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R); |
---|
117 | } |
---|
118 | |
---|
119 | //**************************************************************************** |
---|
120 | // collider function: |
---|
121 | // this collides a transformed geom with another geom. the other geom can |
---|
122 | // also be a transformed geom, but this case is not handled specially. |
---|
123 | |
---|
124 | int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, |
---|
125 | dContactGeom *contact, int skip) |
---|
126 | { |
---|
127 | dIASSERT (skip >= (int)sizeof(dContactGeom)); |
---|
128 | dIASSERT (o1->type == dGeomTransformClass); |
---|
129 | |
---|
130 | dxGeomTransform *tr = (dxGeomTransform*) o1; |
---|
131 | if (!tr->obj) return 0; |
---|
132 | dUASSERT (tr->obj->parent_space==0, |
---|
133 | "GeomTransform encapsulated object must not be in a space"); |
---|
134 | dUASSERT (tr->obj->body==0, |
---|
135 | "GeomTransform encapsulated object must not be attached " |
---|
136 | "to a body"); |
---|
137 | |
---|
138 | // backup the relative pos and R pointers of the encapsulated geom object, |
---|
139 | // and the body pointer |
---|
140 | dxPosR *posr_bak = tr->obj->final_posr; |
---|
141 | dxBody *bodybak = tr->obj->body; |
---|
142 | |
---|
143 | // compute temporary pos and R for the encapsulated geom object. |
---|
144 | // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag, |
---|
145 | // because computeFinalTx() will have already been called in |
---|
146 | // dxGeomTransform::computeAABB() |
---|
147 | |
---|
148 | if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx(); |
---|
149 | tr->obj->final_posr = &tr->transform_posr; |
---|
150 | tr->obj->body = o1->body; |
---|
151 | |
---|
152 | // do the collision |
---|
153 | int n = dCollide (tr->obj,o2,flags,contact,skip); |
---|
154 | |
---|
155 | // if required, adjust the 'g1' values in the generated contacts so that |
---|
156 | // thay indicated the GeomTransform object instead of the encapsulated |
---|
157 | // object. |
---|
158 | if (tr->infomode) { |
---|
159 | for (int i=0; i<n; i++) { |
---|
160 | dContactGeom *c = CONTACT(contact,skip*i); |
---|
161 | c->g1 = o1; |
---|
162 | } |
---|
163 | } |
---|
164 | |
---|
165 | // restore the pos, R and body |
---|
166 | tr->obj->final_posr = posr_bak; |
---|
167 | tr->obj->body = bodybak; |
---|
168 | return n; |
---|
169 | } |
---|
170 | |
---|
171 | //**************************************************************************** |
---|
172 | // public API |
---|
173 | |
---|
174 | dGeomID dCreateGeomTransform (dSpaceID space) |
---|
175 | { |
---|
176 | return new dxGeomTransform (space); |
---|
177 | } |
---|
178 | |
---|
179 | |
---|
180 | void dGeomTransformSetGeom (dGeomID g, dGeomID obj) |
---|
181 | { |
---|
182 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
183 | "argument not a geom transform"); |
---|
184 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
185 | if (tr->obj && tr->cleanup) delete tr->obj; |
---|
186 | tr->obj = obj; |
---|
187 | } |
---|
188 | |
---|
189 | |
---|
190 | dGeomID dGeomTransformGetGeom (dGeomID g) |
---|
191 | { |
---|
192 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
193 | "argument not a geom transform"); |
---|
194 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
195 | return tr->obj; |
---|
196 | } |
---|
197 | |
---|
198 | |
---|
199 | void dGeomTransformSetCleanup (dGeomID g, int mode) |
---|
200 | { |
---|
201 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
202 | "argument not a geom transform"); |
---|
203 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
204 | tr->cleanup = mode; |
---|
205 | } |
---|
206 | |
---|
207 | |
---|
208 | int dGeomTransformGetCleanup (dGeomID g) |
---|
209 | { |
---|
210 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
211 | "argument not a geom transform"); |
---|
212 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
213 | return tr->cleanup; |
---|
214 | } |
---|
215 | |
---|
216 | |
---|
217 | void dGeomTransformSetInfo (dGeomID g, int mode) |
---|
218 | { |
---|
219 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
220 | "argument not a geom transform"); |
---|
221 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
222 | tr->infomode = mode; |
---|
223 | } |
---|
224 | |
---|
225 | |
---|
226 | int dGeomTransformGetInfo (dGeomID g) |
---|
227 | { |
---|
228 | dUASSERT (g && g->type == dGeomTransformClass, |
---|
229 | "argument not a geom transform"); |
---|
230 | dxGeomTransform *tr = (dxGeomTransform*) g; |
---|
231 | return tr->infomode; |
---|
232 | } |
---|