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);
}
