[216] | 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 | } |
---|