void dGeomSetBody (dxGeom *g, dxBody *b) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (b) { if (!g->body) dFree (g->pos,sizeof(dxPosR)); g->pos = b->pos; g->R = b->R; dGeomMoved (g); if (g->body != b) { g->bodyRemove(); g->bodyAdd (b); } } else { if (g->body) { dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); g->pos = pr->pos; g->R = pr->R; memcpy (g->pos,g->body->pos,sizeof(dVector3)); memcpy (g->R,g->body->R,sizeof(dMatrix3)); g->bodyRemove(); } // dGeomMoved() should not be called if the body is being set to 0, as the // new position of the geom is set to the old position of the body, so the // effective position of the geom remains unchanged. } }
void dGeomRaySetLength (dGeomID g, dReal length) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); dxRay *r = (dxRay*) g; r->length = length; dGeomMoved (g); }
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (g->offset_posr) { // move body such that body+offset = position dVector3 world_offset; dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos); dBodySetPosition(g->body, x - world_offset[0], y - world_offset[1], z - world_offset[2]); } else if (g->body) { // this will call dGeomMoved (g), so we don't have to dBodySetPosition (g->body,x,y,z); } else { g->final_posr->pos[0] = x; g->final_posr->pos[1] = y; g->final_posr->pos[2] = z; dGeomMoved (g); } }
void dxQuadTreeSpace::remove(dxGeom* g) { CHECK_NOT_LOCKED(this); dAASSERT(g); dUASSERT(g->parent_space == this,"object is not in this space"); // remove ((Block*)g->tome)->DelObject(g); count--; for (int i = 0; i < DirtyList.size(); i++) { if (DirtyList[i] == g) { DirtyList.remove(i); break; } } // safeguard g->next = 0; g->tome = 0; g->parent_space = 0; // enumerator has been invalidated current_geom = 0; // the bounding box of this space (and that of all the parents) may have // changed as a consequence of the removal. dGeomMoved(this); }
EXPORT_C void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) { dxCapsule *c = (dxCapsule*) g; c->radius = radius; c->lz = length; dGeomMoved (g); }
void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) { dAASSERT (g && quat); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (g->offset_posr) { g->recomputePosr(); // move body such that body+offset = rotation dxPosR new_final_posr; dxPosR new_body_posr; dQtoR (quat, new_final_posr.R); memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); dBodySetRotation(g->body, new_body_posr.R); dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); } if (g->body) { // this will call dGeomMoved (g), so we don't have to dBodySetQuaternion (g->body,quat); } else { dQtoR (quat, g->final_posr->R); dGeomMoved (g); } }
void dxQuadTreeSpace::remove(dxGeom* g){ CHECK_NOT_LOCKED(this); dAASSERT(g); dUASSERT(g->parent_space == this,"object is not in this space"); // remove ((Block*)g->tome)->DelObject(g); count--; for (int i = 0; i < DirtyList.size(); i++){ if (DirtyList[i] == g){ DirtyList.remove(i); // (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i --i; } } // safeguard g->next = 0; g->tome = 0; g->parent_space = 0; // enumerator has been invalidated current_geom = 0; // the bounding box of this space (and that of all the parents) may have // changed as a consequence of the removal. dGeomMoved(this); }
void dGeomSphereSetRadius (dGeomID g, dReal radius) { dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); dAASSERT (radius > 0); dxSphere *s = (dxSphere*) g; s->radius = radius; dGeomMoved (g); }
EXPORT_C void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) { dxBox *b = (dxBox*) g; b->side[0] = lx; b->side[1] = ly; b->side[2] = lz; dGeomMoved (g); }
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) { dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); dAASSERT (radius > 0 && length > 0); dxCapsule *c = (dxCapsule*) g; c->radius = radius; c->lz = length; dGeomMoved (g); }
void dGeomRayMotionsSet (dGeomID g,const dReal* p,const dReal* d, dReal l) { dxRayMotions *c = (dxRayMotions*) dGeomGetClassData(g); dGeomRaySetLength (c->ray, l); dGeomRaySet (c->ray, p[0], p[1], p[2], d[0], d[1], d[2]); dGeomMoved(g); }
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) { dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); dAASSERT (radius >= 0 && length >= 0); dxCapsule *c = (dxCapsule*) g; c->radius = radius; c->lz = length; c->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/); dGeomMoved (g); }
void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length) { dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); dAASSERT (radius >= 0 && length >= 0); dxCylinder *c = (dxCylinder*) cylinder; c->radius = radius; c->lz = length; c->updateZeroSizedFlag(!radius || !length); dGeomMoved (cylinder); }
void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length) { dUASSERT (g && g->type == dConeClass,"argument not a cone"); dAASSERT (_radius > 0.f); dAASSERT (_length > 0.f); dxCone *c = (dxCone*) g; c->radius = _radius; c->lz = _length; dGeomMoved (g); }
void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) { dUASSERT (g && g->type == dBoxClass,"argument not a box"); dAASSERT (lx >= 0 && ly >= 0 && lz >= 0); dxBox *b = (dxBox*) g; b->side[0] = lx; b->side[1] = ly; b->side[2] = lz; b->updateZeroSizedFlag(!lx || !ly || !lz); dGeomMoved (g); }
void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) { dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); dxPlane *p = (dxPlane*) g; p->p[0] = a; p->p[1] = b; p->p[2] = c; p->p[3] = d; make_sure_plane_normal_has_unit_length (p); dGeomMoved (g); }
void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); dUASSERT (g->body, "geom must be on a body"); CHECK_NOT_LOCKED (g->parent_space); if (!g->offset_posr) { dGeomCreateOffset(g); } dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos); dGeomMoved (g); }
void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat) { dAASSERT (g && quat); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); dUASSERT (g->body, "geom must be on a body"); CHECK_NOT_LOCKED (g->parent_space); if (!g->offset_posr) { dGeomCreateOffset (g); } dQtoR (quat, g->offset_posr->R); dGeomMoved (g); }
void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R) { dAASSERT (g && R); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); dUASSERT (g->body, "geom must be on a body"); CHECK_NOT_LOCKED (g->parent_space); if (!g->offset_posr) { dGeomCreateOffset (g); } memcpy (g->offset_posr->R,R,sizeof(dMatrix3)); dGeomMoved (g); }
void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) { dAASSERT (g && quat); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (g->body) { // this will call dGeomMoved (g), so we don't have to dBodySetQuaternion (g->body,quat); } else { dQtoR (quat, g->R); dGeomMoved (g); } }
void dGeomSetRotation (dxGeom *g, const dMatrix3 R) { dAASSERT (g && R); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (g->body) { // this will call dGeomMoved (g), so we don't have to dBodySetRotation (g->body,R); } else { memcpy (g->R,R,sizeof(dMatrix3)); dGeomMoved (g); } }
void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); dUASSERT (g->body, "geom must be on a body"); CHECK_NOT_LOCKED (g->parent_space); if (!g->offset_posr) { dGeomCreateOffset(g); } g->offset_posr->pos[0] = x; g->offset_posr->pos[1] = y; g->offset_posr->pos[2] = z; dGeomMoved (g); }
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (g->body) { // this will call dGeomMoved (g), so we don't have to dBodySetPosition (g->body,x,y,z); } else { g->pos[0] = x; g->pos[1] = y; g->pos[2] = z; dGeomMoved (g); } }
void dxQuadTreeSpace::add(dxGeom* g){ CHECK_NOT_LOCKED (this); dAASSERT(g); dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space"); g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; DirtyList.push(g); // add g->parent_space = this; Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block count++; // enumerator has been invalidated current_geom = 0; dGeomMoved(this); }
void dxSAPSpace::add( dxGeom* g ) { CHECK_NOT_LOCKED (this); dAASSERT(g); dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space"); g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; // add to dirty list GEOM_SET_DIRTY_IDX( g, DirtyList.size() ); GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX ); DirtyList.push( g ); g->parent_space = this; this->count++; dGeomMoved(this); }
void dGeomClearOffset(dxGeom *g) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); if (g->offset_posr) { dIASSERT(g->body); // no longer need an offset posr dFreePosr(g->offset_posr); g->offset_posr = 0; // the geom will now share the position of the body dFreePosr(g->final_posr); g->final_posr = &g->body->posr; // geom has moved g->gflags &= ~GEOM_POSR_BAD; dGeomMoved (g); } }
void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R) { dAASSERT (g && R); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); dUASSERT (g->body, "geom must be on a body"); CHECK_NOT_LOCKED (g->parent_space); if (!g->offset_posr) { dGeomCreateOffset (g); } g->recomputePosr(); dxPosR new_final_posr; memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); memcpy(new_final_posr.R, R, sizeof(dMatrix3)); getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); dGeomMoved (g); }
void dGeomSetBody (dxGeom *g, dxBody *b) { dAASSERT (g); dUASSERT (b == NULL || (g->gflags & GEOM_PLACEABLE),"geom must be placeable"); CHECK_NOT_LOCKED (g->parent_space); if (b) { if (!g->body) dFreePosr(g->final_posr); if (g->body != b) { if (g->offset_posr) { dFreePosr(g->offset_posr); g->offset_posr = 0; } g->final_posr = &b->posr; g->bodyRemove(); g->bodyAdd (b); } dGeomMoved (g); } else { if (g->body) { if (g->offset_posr) { // if we're offset, we already have our own final position, make sure its updated g->recomputePosr(); dFreePosr(g->offset_posr); g->offset_posr = 0; } else { g->final_posr = dAllocPosr(); memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3)); memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3)); } g->bodyRemove(); } // dGeomMoved() should not be called if the body is being set to 0, as the // new position of the geom is set to the old position of the body, so the // effective position of the geom remains unchanged. } }
void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); g->recomputePosr(); dReal* rot = g->final_posr->R; dReal* pos = g->final_posr->pos; dVector3 n; pos[0] = px; pos[1] = py; pos[2] = pz; n[0] = dx; n[1] = dy; n[2] = dz; dNormalize3(n); rot[0*4+2] = n[0]; rot[1*4+2] = n[1]; rot[2*4+2] = n[2]; dGeomMoved (g); }
void dxSAPSpace::remove( dxGeom* g ) { CHECK_NOT_LOCKED(this); dAASSERT(g); dUASSERT(g->parent_space == this,"object is not in this space"); // remove int dirtyIdx = GEOM_GET_DIRTY_IDX(g); int geomIdx = GEOM_GET_GEOM_IDX(g); // must be in one list, not in both dUASSERT( dirtyIdx==GEOM_INVALID_IDX && geomIdx>=0 && geomIdx<GeomList.size() || geomIdx==GEOM_INVALID_IDX && dirtyIdx>=0 && dirtyIdx<DirtyList.size(), "geom indices messed up" ); if( dirtyIdx != GEOM_INVALID_IDX ) { // we're in dirty list, remove int dirtySize = DirtyList.size(); dxGeom* lastG = DirtyList[dirtySize-1]; DirtyList[dirtyIdx] = lastG; GEOM_SET_DIRTY_IDX(lastG,dirtyIdx); GEOM_SET_DIRTY_IDX(g,GEOM_INVALID_IDX); DirtyList.setSize( dirtySize-1 ); } else { // we're in geom list, remove int geomSize = GeomList.size(); dxGeom* lastG = GeomList[geomSize-1]; GeomList[geomIdx] = lastG; GEOM_SET_GEOM_IDX(lastG,geomIdx); GEOM_SET_GEOM_IDX(g,GEOM_INVALID_IDX); GeomList.setSize( geomSize-1 ); } count--; // safeguard g->parent_space = 0; // the bounding box of this space (and that of all the parents) may have // changed as a consequence of the removal. dGeomMoved(this); }