Simple ray casting query

Unlike most physics engines, ODE has no built-in ray casting query function. However, you can easily create one using dSpaceCollide2. Other modifications could be made to include the hit geometry, for example.

const int MAX_CONTACTS = 32;

// Check ray collision against a space
void RayCallback(void *Data, dGeomID Geometry1, dGeomID Geometry2) {
    dReal *HitPosition = (dReal *)Data;

    // Check collisions
    dContact Contacts[MAX_CONTACTS];
    int Count = dCollide(Geometry1, Geometry2, MAX_CONTACTS, &Contacts[0].geom, sizeof(dContact));
    for(int i = 0; i < Count; i++) {

        // Check depth against current closest hit
        if(Contacts[i].geom.depth < HitPosition[3]) {
            HitPosition[0] = Contacts[i].geom.pos[0];
            HitPosition[1] = Contacts[i].geom.pos[1];
            HitPosition[2] = Contacts[i].geom.pos[2];
            HitPosition[3] = Contacts[i].geom.depth;
        }
    }
}

// Performs raycasting on a space and returns the point of collision. Return false for no hit.
bool RaycastQuery(dSpaceID Space, const dVector3 Start, dVector3 End) {
     
    // Calculate direction
    dVector3 Direction;
    Direction[0] = End[0] - Start[0];
    Direction[1] = End[1] - Start[1];
    Direction[2] = End[2] - Start[2];

    // Get length
    dReal Length = dSqrt(Direction[0] * Direction[0] + Direction[1] * Direction[1] + Direction[2] * Direction[2]);
    dReal InverseLength = dRecip(Length);

    // Normalize
    Direction[0] *= InverseLength;
    Direction[1] *= InverseLength;
    Direction[2] *= InverseLength;

    // Create ray
    dGeomID Ray = dCreateRay(0, Length);
    dGeomRaySet(Ray, Start[0], Start[1], Start[2], Direction[0], Direction[1], Direction[2]);
    dGeomSetCategoryBits(Ray, 0);
    dGeomSetCollideBits(Ray, _Physics::FILTER_CAMERA);

    // Check collisions
    dVector4 HitPosition;
    HitPosition[3] = dInfinity;
    dSpaceCollide2(Ray, (dGeomID)Space, HitPosition, &RayCallback);

    // Cleanup
    dGeomDestroy(Ray);

    // Check for hit
    if(HitPosition[3] != dInfinity) {
        End[0] = HitPosition[0];
        End[1] = HitPosition[1];
        End[2] = HitPosition[2];

        return true;
    }

    return false;
}