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 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); 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); }
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 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::add(dxGeom* g){ CHECK_NOT_LOCKED (this); dAASSERT(g); dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space"); DirtyList.push(g); Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block dxSpace::add(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 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 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 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 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_ex)->DelObject(g); 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; } } dxSpace::remove(g); }
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 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 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); }
void dGeomSetCollideBits (dxGeom *g, unsigned long bits) { dAASSERT (g); CHECK_NOT_LOCKED (g->parent_space); g->collide_bits = bits; }
void dGeomSetCategoryBits (dxGeom *g, unsigned long bits) { dAASSERT (g); CHECK_NOT_LOCKED (g->parent_space); g->category_bits = bits; }