std::vector<RaycastResult> & ODESimulator::internal_fireRay(const Rayr& r,
real length, const Solid* attachedSolid,
unsigned int rayContactGroup)
{
Point3r origin = r.getOrigin();
Vec3r dir = r.getDir().unit();
mRaycastResults.clear();
mSensorSolid = attachedSolid;
mRayContactGroup = rayContactGroup;
// Create an ODE ray geom. Make sure its user data pointer is
// NULL because this is used in the collision callback to
// distinguish the ray from other geoms.
dGeomID rayGeomID = dCreateRay(mRootSpaceID, length);
dGeomRaySet(rayGeomID, origin[0], origin[1], origin[2], dir[0],
dir[1], dir[2]);
dGeomSetData(rayGeomID, NULL);
// Check for collisions. This will fill mRaycastResult with valid
// data. Its Solid pointer will remain NULL if nothing was hit.
dSpaceCollide2(rayGeomID, (dGeomID) mRootSpaceID, this,
&ode_hidden::internal_raycastCollisionCallback);
// Finished with ODE ray, so destroy it.
dGeomDestroy(rayGeomID);
return mRaycastResults;
}
bool CDynamics3DEntity::CheckIntersectionWithRay(Real& f_t_on_ray,
const CRay& c_ray) const {
/* Create an ODE ray from ARGoS ray */
Real fRayLength = c_ray.GetLength();
dGeomID tRay = dCreateRay(m_cEngine.GetSpaceID(), fRayLength);
CVector3 cDirection;
c_ray.GetDirection(cDirection);
dGeomRaySet(tRay,
c_ray.GetStart().GetX(),
c_ray.GetStart().GetY(),
c_ray.GetStart().GetZ(),
cDirection.GetX(),
cDirection.GetY(),
cDirection.GetZ());
/* Create the structure to contain info about the possible
ray/geom intersection */
dContactGeom tIntersection;
/* Check for intersection between the ray and the object local space */
if(dCollide(tRay, reinterpret_cast<dGeomID>(m_tEntitySpace), 1, &tIntersection, sizeof(dContactGeom)) > 0) {
/* There is an intersecton */
f_t_on_ray = tIntersection.depth / fRayLength;
return true;
}
else {
/* No intersection detected */
return false;
}
}
Body* SimObjectRenderer::selectObject(const Vector3<>& projectedClick)
{
if(&simObject != Simulation::simulation->scene)
return nullptr;
class Callback
{
public:
Body* closestBody;
float closestSqrDistance;
const Vector3<>& cameraPos;
Callback(const Vector3<>& cameraPos) : closestBody(0), cameraPos(cameraPos) {}
static void staticCollisionCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
{
ASSERT(!dGeomIsSpace(geom1));
ASSERT(!dGeomIsSpace(geom2));
ASSERT(dGeomGetBody(geom1) || dGeomGetBody(geom2));
dContact contact[1];
if(dCollide(geom1, geom2, 1, &contact[0].geom, sizeof(dContact)) < 1)
return;
dGeomID geom = geom2;
dBodyID bodyId = dGeomGetBody(geom2);
if(!bodyId)
{
bodyId = dGeomGetBody(geom1);
geom = geom1;
}
const dReal* pos = dGeomGetPosition(geom);
float sqrDistance = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - callback->cameraPos).squareAbs();
if(!callback->closestBody || sqrDistance < callback->closestSqrDistance)
{
callback->closestBody = (Body*)dBodyGetData(bodyId);
callback->closestSqrDistance = sqrDistance;
}
}
static void staticCollisionWithSpaceCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
{
ASSERT(!dGeomIsSpace(geom1));
ASSERT(dGeomIsSpace(geom2));
dSpaceCollide2(geom1, geom2, callback, (dNearCallback*)&staticCollisionCallback);
}
};
Callback callback(cameraPos);
dGeomID ray = dCreateRay(Simulation::simulation->staticSpace, 10000.f);
Vector3<> dir = projectedClick - cameraPos;
dGeomRaySet(ray, cameraPos.x, cameraPos.y, cameraPos.z, dir.x, dir.y, dir.z);
dSpaceCollide2(ray, (dGeomID)Simulation::simulation->movableSpace, &callback, (dNearCallback*)&Callback::staticCollisionWithSpaceCallback);
dGeomDestroy(ray);
if(!callback.closestBody)
return nullptr;
Body* body = callback.closestBody;
return body->rootBody;
}
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);
}
/*
Test rays within the cylinder
-completely inside
-exiting through side
-exiting through cap
-exiting through corner
Test rays outside the cylinder
*/
int test_ray_and_cylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
const int positions = 5;
const int slices = 13;
const int pitch = 11;
dSimpleSpace space(0);
dGeomID ray = dCreateRay(space,4);
dGeomID cyl = dCreateCylinder(space,0.5,1);
// The first thing that happens is the ray is
// rotated into cylinder coordinates. We'll trust that's
// done right. The major axis is in the z-dir.
// Random tests
/*b[0]=4*dRandReal()-2;
b[1]=4*dRandReal()-2;
b[2]=4*dRandReal()-2;
a[0]=2*dRandReal()-1;
a[1]=2*dRandReal()-1;
a[2]=2*dRandReal()-1;*/
// Inside out
b[0]=dRandReal()-0.5;
b[1]=dRandReal()-0.5;
b[2]=dRandReal()-0.5;
a[0]=2*dRandReal()-1;
a[1]=2*dRandReal()-1;
a[2]=2*dRandReal()-1;
// Outside in
/*b[0]=4*dRandReal()-2;
b[1]=4*dRandReal()-2;
b[2]=4*dRandReal()-2;
a[0]=-b[0];
a[1]=-b[1];
a[2]=-b[2];*/
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
// This is just for visual inspection right now.
//if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
draw_all_objects (space);
PASSED();
}
dGeomID set_phys_geom_type(dGeomID geom, dBodyID body,
int i, int t, const float *v)
{
/* Destroy the old geom and its data. */
if (geom)
{
free(dGeomGetData(geom));
dGeomDestroy(geom);
geom = 0;
}
/* Create a new geom of the required type. */
switch (t)
{
case dSphereClass:
geom = dCreateSphere(space, v[0]);
break;
case dCapsuleClass:
geom = dCreateCapsule(space, v[0], v[1]);
break;
case dBoxClass:
geom = dCreateBox(space, v[0], v[1], v[2]);
break;
case dPlaneClass:
geom = dCreatePlane(space, v[0], v[1], v[2], v[3]);
break;
case dRayClass:
geom = dCreateRay(space, (dReal) sqrt(v[3] * v[3] +
v[4] * v[4] +
v[5] * v[5]));
dGeomRaySet(geom, v[0], v[1], v[2], v[3], v[4], v[5]);
break;
}
/* Assign geom data and attach it to the body. */
if (geom)
{
dGeomSetData(geom, create_data(i));
dGeomSetBody(geom, body);
}
return geom;
}
void SingleDistanceSensor::DistanceSensor::updateValue()
{
pose = physicalObject->pose;
pose.conc(offset);
const Vector3<>& pos = pose.translation;
const Vector3<>& dir = pose.rotation.c0;
dGeomRaySet(geom, pos.x, pos.y, pos.z, dir.x, dir.y, dir.z);
closestGeom = 0;
closestSqrDistance = maxSqrDist;
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
if(closestGeom)
{
data.floatValue = sqrtf(closestSqrDistance);
if(data.floatValue < min)
data.floatValue = min;
}
else
data.floatValue = max;
}
void ApproxDistanceSensor::DistanceSensor::staticCollisionCallback(ApproxDistanceSensor::DistanceSensor* sensor, dGeomID geom1, dGeomID geom2)
{
ASSERT(geom1 == sensor->geom);
ASSERT(!dGeomIsSpace(geom2));
Geometry* geometry = (Geometry*)dGeomGetData(geom2);
if((::PhysicalObject*)geometry->parentBody == sensor->physicalObject)
return; // avoid detecting the body on which the sensor is monted
const dReal* pos = dGeomGetPosition(geom2);
const Vector3<> geomPos((float) pos[0], (float) pos[1], (float) pos[2]);
const float approxSqrDist = (geomPos - sensor->pose.translation).squareAbs() - geometry->innerRadiusSqr;
if(approxSqrDist >= sensor->closestSqrDistance)
return; // we already found another geometrie that was closer
Vector3<> relPos = sensor->invertedPose * geomPos;
if(relPos.x <= 0.f)
return; // center of the geometry should be in front of the distance sensor
float halfMaxY = sensor->tanHalfAngleX * relPos.x;
float halfMaxZ = sensor->tanHalfAngleY * relPos.x;
if(std::max(std::abs(relPos.y) - geometry->outerRadius, 0.f) >= halfMaxY || std::max(std::abs(relPos.z) - geometry->outerRadius, 0.f) >= halfMaxZ)
return; // the sphere that covers the geometrie does not collide with the pyramid of the distance sensor
if(std::max(std::abs(relPos.y) - geometry->innerRadius, 0.f) < halfMaxY && std::max(std::abs(relPos.z) - geometry->innerRadius, 0.f) < halfMaxZ)
goto hit; // the sphere enclosed by the geometrie collides with the pyramid of the distance sensor
// geom2 might collide with the pyramid of the distance sensor. let us perform a hit scan along one of the pyramid's sides to find out..
{
Vector3<> scanDir = sensor->pose.rotation * Vector3<>(relPos.x, std::max(std::min(relPos.y, halfMaxY), -halfMaxY), std::max(std::min(relPos.z, halfMaxZ), -halfMaxZ));
const Vector3<>& sensorPos = sensor->pose.translation;
dGeomRaySet(sensor->scanRayGeom, sensorPos.x, sensorPos.y, sensorPos.z, scanDir.x, scanDir.y, scanDir.z);
dContactGeom contactGeom;
if(dCollide(sensor->scanRayGeom, geom2, CONTACTS_UNIMPORTANT | 1, &contactGeom, sizeof(dContactGeom)) <= 0)
return;
}
hit:
sensor->closestSqrDistance = approxSqrDist;
sensor->closestGeom = geom2;
}
void LaserBeam::createGeometry(const dSpaceID& space)
{
/// build a ray
Model::LaserBeam* beam = getTrait<Model::LaserBeam>() ;
Model::Mobile* mobile = getTrait<Model::Mobile>() ;
float collision_beam_length =
std::max(mobile->getSpeed().MeterPerSecond().length()*getControlerSet()->getTimeStep(),
beam->getLength().Meter()) ;
m_ray = dCreateRay(0,2*collision_beam_length) ;
/// we use -z as forward @see Model::Orientation
dGeomRaySet(m_ray,0,0,0,0,0,-1) ;
dGeomRaySetParams(m_ray,false,false) ;
dGeomRaySetClosestHit(m_ray,true) ;
/// a geometry transform allows to have a local z axis inversion
m_geometry1 = dCreateGeomTransform(space) ;
dGeomTransformSetGeom(m_geometry1,m_ray) ;
dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Laser) ;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE();
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
//dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
Size[0] = 5.0f;
Size[1] = 5.0f;
Size[2] = 2.5f;
Vertices[0][0] = -Size[0];
Vertices[0][1] = -Size[1];
Vertices[0][2] = Size[2];
Vertices[1][0] = Size[0];
Vertices[1][1] = -Size[1];
Vertices[1][2] = Size[2];
Vertices[2][0] = Size[0];
Vertices[2][1] = Size[1];
Vertices[2][2] = Size[2];
Vertices[3][0] = -Size[0];
Vertices[3][1] = Size[1];
Vertices[3][2] = Size[2];
Vertices[4][0] = 0;
Vertices[4][1] = 0;
Vertices[4][2] = 0;
Indices[0] = 0;
Indices[1] = 1;
Indices[2] = 4;
Indices[3] = 1;
Indices[4] = 2;
Indices[5] = 4;
Indices[6] = 2;
Indices[7] = 3;
Indices[8] = 4;
Indices[9] = 3;
Indices[10] = 0;
Indices[11] = 4;
dTriMeshDataID Data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount);
TriMesh = dCreateTriMesh(space, Data, 0, 0, 0);
//dGeomSetPosition(TriMesh, 0, 0, 1.0);
Ray = dCreateRay(space, 0.9);
dVector3 Origin, Direction;
Origin[0] = 0.0;
Origin[1] = 0;
Origin[2] = 0.5;
Origin[3] = 0;
Direction[0] = 0;
Direction[1] = 1.1f;
Direction[2] = -1;
Direction[3] = 0;
dNormalize3(Direction);
dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void PRay::setPose(dReal x,dReal y,dReal z,dReal dx,dReal dy,dReal dz)
{
dGeomRaySet(geom,x,y,z,dx,dy,dz);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
//dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
Size[0] = 5.0f;
Size[1] = 5.0f;
Size[2] = 2.5f;
Vertices[0][0] = -Size[0];
Vertices[0][1] = -Size[1];
Vertices[0][2] = Size[2];
Vertices[1][0] = Size[0];
Vertices[1][1] = -Size[1];
Vertices[1][2] = Size[2];
Vertices[2][0] = Size[0];
Vertices[2][1] = Size[1];
Vertices[2][2] = Size[2];
Vertices[3][0] = -Size[0];
Vertices[3][1] = Size[1];
Vertices[3][2] = Size[2];
Vertices[4][0] = 0;
Vertices[4][1] = 0;
Vertices[4][2] = 0;
Indices[0] = 0;
Indices[1] = 1;
Indices[2] = 4;
Indices[3] = 1;
Indices[4] = 2;
Indices[5] = 4;
Indices[6] = 2;
Indices[7] = 3;
Indices[8] = 4;
Indices[9] = 3;
Indices[10] = 0;
Indices[11] = 4;
dTriMeshDataID Data = dGeomTriMeshDataCreate();
//dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount);
dGeomTriMeshDataBuildSingle(Data, Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(Data, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
TriMesh = dCreateTriMesh(space, Data, 0, 0, 0);
//dGeomSetPosition(TriMesh, 0, 0, 1.0);
Ray = dCreateRay(space, 0.9);
dVector3 Origin, Direction;
Origin[0] = 0.0;
Origin[1] = 0;
Origin[2] = 0.5;
Origin[3] = 0;
Direction[0] = 0;
Direction[1] = 1.1f;
Direction[2] = -1;
Direction[3] = 0;
dNormalize3(Direction);
dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
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 main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
world = dWorldCreate();
space = dHashSpaceCreate();
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
dVector3 Origin, Direction;
RayCount = 5;
Rays = new dGeomID[RayCount];
/* Ray 0 */
Origin[0] = 1;
Origin[1] = 1;
Origin[2] = 1.5;
Origin[3] = 0;
Direction[0] = 0.0f;
Direction[1] = 0.0f;
Direction[2] = -1;
Direction[3] = 0;
dNormalize3(Direction);
Rays[0] = dGeomCreateRay(space, 5.0f);
dGeomRaySet(Rays[0], Origin, Direction);
/* Ray 1 */
Origin[0] = 0;
Origin[1] = 10;
Origin[2] = 0.25;
Origin[3] = 0;
Direction[0] = 0.0f;
Direction[1] = -1.0f;
Direction[2] = 0.0f;
Direction[3] = 0;
dNormalize3(Direction);
Rays[1] = dGeomCreateRay(space, 20.0f);
dGeomRaySet(Rays[1], Origin, Direction);
/* Ray 2 */
Origin[0] = -10;
Origin[1] = 0;
Origin[2] = 0.20;
Origin[3] = 0;
Direction[0] = 1.0f;
Direction[1] = 0.0f;
Direction[2] = 0.0f;
Direction[3] = 0;
dNormalize3(Direction);
Rays[2] = dGeomCreateRay(space, 20.0f);
dGeomRaySet(Rays[2], Origin, Direction);
/* Ray 3 */
Origin[0] = -9;
Origin[1] = 11;
Origin[2] = 0.15;
Origin[3] = 0;
Direction[0] = 1.0f;
Direction[1] = -1.0f;
Direction[2] = 0.0f;
Direction[3] = 0;
dNormalize3(Direction);
Rays[3] = dGeomCreateRay(space, 20.0f);
dGeomRaySet(Rays[3], Origin, Direction);
/* Ray 4 */
Origin[0] = -0.1;
Origin[1] = 0.3;
Origin[2] = 0.30;
Origin[3] = 0;
Direction[0] = 0.3f;
Direction[1] = 0.5f;
Direction[2] = 1.0f;
Direction[3] = 0;
Rays[4] = dGeomCreateRay(space, 5.0f);
dGeomRaySet(Rays[4], Origin, Direction);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
int test_ray_and_sphere()
{
int j;
dContactGeom contact;
dVector3 p,q,q2,n,v1;
dMatrix3 R;
dReal r,k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test zero length ray just inside sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside that sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal();
for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
dMakeRandomVector (q2,3,1.0);
dNormalize3 (q2);
k = dRandReal();
for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
for (j=0; j<3; j++) n[j] = q2[j] - q[j];
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2));
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
do {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
}
while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) n[j] = -q[j];
for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,0.99*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,1.01*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED();
// ********** test contact point distance for random rays
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal()+0.5;
for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere));
if (dFabs(k - r) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
// also check depth of contact point
if (dFabs (dGeomSpherePointDepth
(sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test tangential grazing - miss
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test tangential grazing - hit
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
PASSED();
}
int test_ray_and_ccylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,ccyl);
// ********** make a random capped cylinder
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test ray completely within ccyl
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
dNormalize3 (b);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
for (j=0; j<3; j++) b[j] -= a[j];
dNormalize3 (b);
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just misses (between caps)
k = dRandReal()*2*M_PI;
x = sin(k);
y = cos(k);
for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (between caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test ray outside ccyl that just misses (caps)
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) < 0) {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
}
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test random rays
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
dNormalize3 (n);
dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ray_and_plane()
{
int j;
dContactGeom contact;
dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
dMatrix3 R;
dReal d;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,ray);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test finite length ray below plane
dGeomRaySetLength (ray,0.09);
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = -dRandReal()*0.5 - 0.1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
dGeomSetPosition (ray,b[0],b[1],b[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray above plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()*0.5 + 0.01;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal() + 0.01;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray that intersects plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()-0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal()-0.5;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
// test that contact is on plane surface
if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED();
// also check contact point depth
if (dFabs (dGeomPlanePointDepth
(plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test ray that just misses
for (j=0; j<3; j++) b[j] = (1+d)*n[j];
for (j=0; j<3; j++) h[j] = -n[j];
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,0.99);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray that just hits
dGeomRaySetLength (ray,1.01);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test polarity with typical ground plane
dGeomPlaneSetParams (plane,0,0,1,0);
for (j=0; j<3; j++) a[j] = 0.1;
for (j=0; j<3; j++) b[j] = 0;
a[2] = 1;
b[2] = -1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
a[2] = -1;
b[2] = 1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
PASSED();
}
