int test_dBoxTouchesBox()
{
int k,bt1,bt2;
dVector3 p1,p2,side1,side2;
dMatrix3 R1,R2;
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
draw_all_objects (space);
int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
bt1 = t1 || t2;
bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
if (bt1 != bt2) FAILED();
/*
// some more debugging info if necessary
if (bt1 && bt2) printf ("agree - boxes touch\n");
if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
"says no\n");
if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
"says yes\n");
*/
PASSED();
}
void CProtoHapticDoc::UpdateDynamics()
{
for(int i= 0; i<m_shapeCount; i++) {
dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dGeomSetPosition (m_geoms[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
dGeomSetRotation (m_geoms[i], dBodyGetRotation(bodies[i]));
dBodySetPosition (bodies[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
float *rotation= m_shapes[i]->getRotation();
const dReal rot[12]=
{ rotation[0], rotation[4], rotation[8], rotation[12],
rotation[1], rotation[5], rotation[9], rotation[13],
rotation[2], rotation[6], rotation[10], rotation[14] };
dBodySetRotation (bodies[i], rot);
dMass mass;
dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dBodySetMass (bodies[i], &mass);
}
}
void BGcubeD_D::BGSetSize(float x, float y, float z) {
h_m = x;
w_m = y;
d_m = z;
dGeomBoxSetLengths(boxGeom_m, h_m, w_m, d_m);
}
IoObject *IoODEBox_setLengths(IoODEBox *self, IoObject *locals, IoMessage *m)
{
dReal lx = IoMessage_locals_doubleArgAt_(m, locals, 0);
dReal ly = IoMessage_locals_doubleArgAt_(m, locals, 1);
dReal lz = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBox_assertHasBoxId(self, locals, m);
dGeomBoxSetLengths(GEOMID, lx, ly, lz);
return self;
}
void cubeD_D::SetSize(float x, float y, float z) {
h_m = x;
w_m = y;
d_m = z;
dMass mass;
dMassSetBox(&mass, density, h_m, w_m, d_m);
dBodySetMass(boxBody_m, &mass);
dGeomBoxSetLengths(boxGeom_m, h_m, w_m, d_m);
}
static void _soy_bodies_box_size_set (soybodiesBox* self, soyatomsSize* size) {
soyatomsSize* _tmp0_;
gfloat _tmp1_;
gfloat _tmp2_;
soyatomsSize* _tmp3_;
gfloat _tmp4_;
gfloat _tmp5_;
soyatomsSize* _tmp6_;
gfloat _tmp7_;
gfloat _tmp8_;
soyscenesScene* _tmp9_;
struct dxGeom* _tmp11_;
GLfloat _tmp12_;
GLfloat _tmp13_;
GLfloat _tmp14_;
soyscenesScene* _tmp15_;
g_return_if_fail (self != NULL);
g_return_if_fail (size != NULL);
g_mutex_lock (&((soybodiesBody*) self)->mutex);
_tmp0_ = size;
_tmp1_ = soy_atoms_size_get_width (_tmp0_);
_tmp2_ = _tmp1_;
self->priv->_width = (GLfloat) _tmp2_;
_tmp3_ = size;
_tmp4_ = soy_atoms_size_get_height (_tmp3_);
_tmp5_ = _tmp4_;
self->priv->_height = (GLfloat) _tmp5_;
_tmp6_ = size;
_tmp7_ = soy_atoms_size_get_depth (_tmp6_);
_tmp8_ = _tmp7_;
self->priv->_depth = (GLfloat) _tmp8_;
self->priv->_updated = TRUE;
g_mutex_unlock (&((soybodiesBody*) self)->mutex);
_tmp9_ = ((soybodiesBody*) self)->scene;
if (_tmp9_ != NULL) {
soyscenesScene* _tmp10_;
_tmp10_ = ((soybodiesBody*) self)->scene;
g_rw_lock_writer_lock (&_tmp10_->stepLock);
}
_tmp11_ = ((soybodiesBody*) self)->geom;
_tmp12_ = self->priv->_width;
_tmp13_ = self->priv->_height;
_tmp14_ = self->priv->_depth;
dGeomBoxSetLengths ((struct dxGeom*) _tmp11_, (dReal) _tmp12_, (dReal) _tmp13_, (dReal) _tmp14_);
_tmp15_ = ((soybodiesBody*) self)->scene;
if (_tmp15_ != NULL) {
soyscenesScene* _tmp16_;
_tmp16_ = ((soybodiesBody*) self)->scene;
g_rw_lock_writer_unlock (&_tmp16_->stepLock);
}
}
int CPhysicAdapter::Modify(INode *node,float tms){
if (myNode!=node) return 0;
const dReal* p_pos = dBodyGetPosition(myPhysObject->myBody);
const dReal* R = dBodyGetRotation(myPhysObject->myBody);
CVector pos = CVector(p_pos[0],p_pos[1],p_pos[2]);
Matrix matrix;
matrix.m_data[0]=R[0];
matrix.m_data[1]=R[4];
matrix.m_data[2]=R[8];
matrix.m_data[3]=0;
matrix.m_data[4]=R[1];
matrix.m_data[5]=R[5];
matrix.m_data[6]=R[9];
matrix.m_data[7]=0;
matrix.m_data[8]=R[2];
matrix.m_data[9]=R[6];
matrix.m_data[10]=R[10];
matrix.m_data[11]=0;
matrix.m_data[12]=p_pos[0];
matrix.m_data[13]=p_pos[1];
matrix.m_data[14]=p_pos[2];
matrix.m_data[15]=1;
//float pitch=p_rot[0]*180.0f/M_PI;
//float yaw=p_rot[1]*180.0f/M_PI;
//float roll=p_rot[2]*180.0f/M_PI;
//printf("%f %f %f\n", pitch,roll,yaw);
//CVector rot = CVector(pitch,yaw,roll);
CVector rot = matrix.GetRot();
rot.v[0]=rot.v[0]*180.0f/M_PI;
rot.v[1]=rot.v[1]*180.0f/M_PI;
rot.v[2]=rot.v[2]*180.0f/M_PI;
myNode->setPos(pos);
myNode->setRot( rot );
CVector size = myNode->getSize();
dGeomBoxSetLengths(myPhysObject->myGeom, size.v[0], size.v[1], size.v[2] );
};
void OscPrismODE::on_size()
{
if (m_size.x <= 0)
m_size.x = 0.0001;
if (m_size.y <= 0)
m_size.y = 0.0001;
if (m_size.z <= 0)
m_size.z = 0.0001;
ODEObject *ode_object = static_cast<ODEObject*>(special());
// resize ODE geom
dGeomBoxSetLengths (ode_object->geom(), m_size[0], m_size[1], m_size[2]);
// reset the mass to maintain same density
dMassSetBox(&ode_object->mass(), m_density.m_value,
m_size[0], m_size[1], m_size[2]);
dBodySetMass(ode_object->body(), &ode_object->mass());
m_mass.m_value = ode_object->mass().mass;
}
bool CPHActivationShape:: Activate (const Fvector need_size,u16 steps,float max_displacement,float max_rotation,bool un_freeze_later/* =false*/)
{
#ifdef DEBUG
if(debug_output().ph_dbg_draw_mask().test(phDbgDrawDeathActivationBox))
{
debug_output().DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;dGeomBoxGetLengths(m_geom,cast_fp(v));v.mul(0.5f);
debug_output().DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,0));
}
#endif
VERIFY(m_geom&&m_body);
CPHObject::activate();
ph_world->Freeze();
UnFreeze();
max_depth=0.f;
dGeomUserDataSetObjectContactCallback(m_geom,GetMaxDepthCallback) ;
//ph_world->Step();
ph_world->StepTouch();
u16 num_it =15;
float fnum_it=float(num_it);
float fnum_steps=float(steps);
float fnum_steps_r=1.f/fnum_steps;
float resolve_depth=0.01f;
float max_vel=max_depth/fnum_it*fnum_steps_r/fixed_step;
float limit_l_vel=_max(_max(need_size.x,need_size.y),need_size.z)/fnum_it*fnum_steps_r/fixed_step;
if(limit_l_vel>default_l_limit)
limit_l_vel=default_l_limit;
if(max_vel>limit_l_vel)
max_vel=limit_l_vel;
float max_a_vel=max_rotation/fnum_it*fnum_steps_r/fixed_step;
if(max_a_vel>default_w_limit)
max_a_vel=default_w_limit;
//ph_world->CutVelocity(0.f,0.f);
dGeomUserDataSetCallbackData(m_geom,this);
dGeomUserDataSetObjectContactCallback( m_geom, ActivateTestDepthCallback );
if( m_flags.test( flStaticEnvironment ) )
dGeomUserDataAddObjectContactCallback(m_geom,StaticEnvironment);
max_depth=0.f;
Fvector from_size;
Fvector step_size,size;
dGeomBoxGetLengths(m_geom,cast_fp(from_size));
step_size.sub(need_size,from_size);
step_size.mul(fnum_steps_r);
size.set(from_size);
bool ret=false;
V_PH_WORLD_STATE temp_state;
ph_world->GetState(temp_state);
for(int m=0;steps>m;++m)
{
//float param =fnum_steps_r*(1+m);
//InterpolateBox(id,param);
size.add(step_size);
dGeomBoxSetLengths(m_geom,size.x,size.y,size.z);
u16 attempts=10;
do{
ret=false;
for(int i=0;num_it>i;++i)
{
max_depth=0.f;
ph_world->Step();
CHECK_POS(Position(),"pos after ph_world->Step()",false);
ph_world->CutVelocity(max_vel,max_a_vel);
CHECK_POS(Position(),"pos after CutVelocity",true);
//if(m==0&&i==0)ph_world->GetState(temp_state);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
attempts--;
}while(!ret&&attempts>0);
#ifdef DEBUG
// Msg("correction attempts %d",10-attempts);
#endif
}
RestoreVelocityState(temp_state);
CHECK_POS(Position(),"pos after RestoreVelocityState(temp_state);",true);
if(!un_freeze_later)ph_world->UnFreeze();
#ifdef DEBUG
if(debug_output().ph_dbg_draw_mask().test(phDbgDrawDeathActivationBox))
{
debug_output().DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;v.set(need_size);v.mul(0.5f);
debug_output().DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,255));
debug_output().DBG_ClosedCashedDraw(30000);
}
#endif
return ret;
}
void Physics::perframe(){
//dWorldStep(worldId);
return; //not implemented yet
#ifdef ODE
for(int i=0; i<level->colliders.size(); i++){
Object* obj=level->colliders[i];
if(level->colliders[i]->collideType=="box" || level->colliders[i]->collideType=="cube"){
if(level->colliders[i]->oldCollideType!="box" && level->colliders[i]->oldCollideType!="cube"){
obj->geomId=dCreateBox(spaceId,obj->collideParameters.x,obj->collideParameters.y,obj->collideParameters.z);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
if(obj->boxBuilt){
float lx=level->colliders[i]->box.px-level->colliders[i]->box.nx;
float ly=level->colliders[i]->box.py-level->colliders[i]->box.ny;
float lz=level->colliders[i]->box.pz-level->colliders[i]->box.nz;
dGeomBoxSetLengths (level->colliders[i]->geomId, lx, ly, lz);
}
}else if(obj->collideType=="sphere"){
if(obj->oldCollideType!="sphere"){
obj->geomId=dCreateSphere(spaceId,obj->collideParameters.x);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
}
}else if(obj->collideType=="plane"){
obj->geomId=dCreatePlane(spaceId,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x);
}else if(obj->collideType=="cylindar"){
obj->geomId=dCreateCCylinder(spaceId,obj->collideParameters.x,obj->collideParameters.y);
}else if(obj->collideType=="triangle"){
if(obj->oldCollideType!="triangle"){
dTriMeshDataID tridat=dGeomTriMeshDataCreate();
obj->geomId=dCreateTriMesh (spaceId, tridat,
NULL,
NULL,
NULL);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
}
if(obj->collide){
dGeomEnable(obj->geomId);
}else{
dGeomDisable(obj->geomId);
}
for(int j=0; j<level->colliders.size(); j++){
if(!level->colliders[i]->geomIdInit || !level->colliders[j]->geomIdInit){
continue;
}
dContactGeom contactg[2];
int cnt=dCollide(level->colliders[i]->geomId,level->colliders[j]->geomId,0,contactg,sizeof(dContactGeom));
if(cnt>0){
dSurfaceParameters surf;
surf.mode=0;
surf.mode=dContactBounce;
surf.mu=level->colliders[i]->friction;
surf.bounce=level->colliders[i]->bounce;
surf.bounce_vel=level->colliders[i]->bounceThreshold;
dContact contact;
contact.geom=contactg[0];
contact.surface=surf;
dJointID joint=dJointCreateContact(worldId,0,&contact);
if(!level->colliders[i]->dynamic){
dJointAttach(joint,level->colliders[j]->bodyId,0);
}else if(!level->colliders[j]->dynamic){
dJointAttach(joint,level->colliders[i]->bodyId,0);
}else{
dJointAttach(joint,level->colliders[i]->bodyId,level->colliders[j]->bodyId);
}
}
}
}
dWorldSetGravity(worldId,gravity.x,gravity.y,gravity.z);
dWorldQuickStep(worldId,1);
for(int i=0; i<level->dynamicObjects.size(); i++){
if(!level->dynamicObjects[i]->bodyIdInit){
continue;
}
if(level->dynamicObjects[i]->dynamic){
dBodyEnable(level->dynamicObjects[i]->bodyId);
}else{
dBodyDisable(level->dynamicObjects[i]->bodyId);
}
const dReal* pos=new dReal[3];
pos=dBodyGetPosition(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->pos.x=pos[0];
level->dynamicObjects[i]->pos.y=pos[1];
level->dynamicObjects[i]->pos.z=pos[2];
const dReal* rot=new dReal[3];
rot=dBodyGetRotation(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->rot.x=rot[0];
level->dynamicObjects[i]->rot.y=rot[1];
level->dynamicObjects[i]->rot.z=rot[2];
}
#endif
}
//! setting of scale
void SSimEntity::setScale(Vector3d scale)
{
m_scale.set(scale.x(), scale.y(), scale.z());
// if a part is already added
int geomNum = getGeomNum();
if (geomNum != 0) {
dBodyID body = m_parts.body;
// loop for all of the parts
for (int i = 0; i < geomNum; i++) {
// refer the geometry
dGeomID geom = m_parts.geoms[i];
// refer the position (gap from CoG)
const dReal *pos = dGeomGetPosition(geom);
// Reflection of scale
dGeomSetPosition(geom, pos[0]*scale.x(), pos[1]*scale.y(), pos[2]*scale.z());
// Refer the type of the geometory
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
// average of scale
double mean = (scale.x() + scale.y() + scale.z())/ 3;
// refer the radius
dReal radius = dGeomSphereGetRadius(geom);
// reflection of scale
dGeomSphereSetRadius(geom, radius*mean);
}
// box
else if (type == 1) {
// refer the size
dVector3 size;
dGeomBoxGetLengths(geom, size);
// reflection of scale
dGeomBoxSetLengths(geom, size[0]*scale.x(), size[1]*scale.y(), size[2]*scale.z());
}
// cylinder
else if (type == 3) {
dReal radius, length;
dGeomCylinderGetParams(geom, &radius, &length);
// average of scale in horizontal plane
double mean = (scale.x() + scale.z()) / 2;
// TODO: confirm: is 2 suitable for long axis?
dGeomCylinderSetParams(geom, radius*mean, length*scale.y());
}
} // for (int i = 0; i < partsNum; i++) {
}
}
int test_ray_and_box()
{
int i,j;
dContactGeom contact;
dVector3 s,p,q,n,q2,q3,q4; // s = box sides
dMatrix3 R;
dReal k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test zero length ray just inside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
for (j=0; j<3; j++) q4[j] += p[j];
for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
dNormalize3 (n);
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
dNormalize3 (q2);
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
for (j=0; j<3; j++) q2[j] = -q2[j];
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,k*0.99);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,k*1.01);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test contact point position for random rays
for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
dNormalize3 (q3);
dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomBoxPointDepth
(box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check position of contact point
for (j=0; j<3; j++) contact.pos[j] -= p[j];
dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
FAILED();
}
// also check normal signs
if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_box_point_depth()
{
int i,j;
dVector3 s,p,q,q2; // s = box sides
dMatrix3 R;
dReal ss,d; // ss = smallest side
dSimpleSpace space(0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test center point has depth of smallest side
ss = 1e9;
for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
// ********** test points outside box have -ve depth
for (j=0; j<3; j++) {
q[j] = 0.5*s[j] + dRandReal() + 0.01;
if (dRandReal() > 0.5) q[j] = -q[j];
}
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
// ********** test points inside box have +ve depth
for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
// ********** test random depth of point aligned along axis (up to ss deep)
i = dRandInt (3);
for (j=0; j<3; j++) q[j] = 0;
d = (dRandReal()*(ss*0.5+1)-1);
q[i] = s[i]*0.5 - d;
if (dRandReal() > 0.5) q[i] = -q[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
PASSED();
}
int test_dBoxBox()
{
int k,bt;
dVector3 p1,p2,side1,side2,normal,normal2;
dMatrix3 R1,R2;
dReal depth,depth2;
int code;
dContactGeom contact[48];
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
// dRSetIdentity (R1); // we can also try this
// dRSetIdentity (R2);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
code = 0;
depth = 0;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
sizeof(dContactGeom));
if (bt==1) {
p2[0] += normal[0] * 0.96 * depth;
p2[1] += normal[1] * 0.96 * depth;
p2[2] += normal[2] * 0.96 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
/*
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
*/
if (bt != 1) {
FAILED();
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
}
p2[0] += normal[0] * 0.08 * depth;
p2[1] += normal[1] * 0.08 * depth;
p2[2] += normal[2] * 0.08 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
if (bt != 0) FAILED();
// dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
// draw_all_objects (space);
}
// printf ("code=%2d depth=%.4f ",code,depth);
PASSED();
}
void CRigidCapsule::setDimentions(const CVector3 &d){
mDimentions = d;
assert(true);
dGeomBoxSetLengths(mGeomID, TO_PHYSICS(d.x), TO_PHYSICS(d.y), TO_PHYSICS(d.z));
}
void CRigidBox::setDimentions(const CVector3 &d){
mDimentions = d;
dGeomBoxSetLengths(mGeomID, TO_PHYSICS(d.x), TO_PHYSICS(d.y), TO_PHYSICS(d.z));
}
