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;
}
}
bool CKinematics2DCollisionCircle::CheckIntersectionWithRay(Real& f_distance, const CRay& c_ray) const {
CSegment c_segment = c_ray.ProjectOntoXY();
CVector2 c_closest_point, c_closest_segment_point;
c_segment.GetMinimumDistancePoints( m_cPosition, c_closest_point, c_closest_segment_point );
Real f_min_segment_distance = Distance(m_cPosition,c_closest_segment_point);
if( f_min_segment_distance > m_fRadius ) {
return false;
}
// see http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
CVector2 dp = c_segment.GetEnd() - c_segment.GetStart();
Real a = dp.SquareLength();
Real b = 2 * dp.DotProduct(c_segment.GetStart() - m_cPosition);
Real c = (m_cPosition.SquareLength() +
c_segment.GetStart().SquareLength() -
2 * m_cPosition.DotProduct(c_segment.GetStart()) -
m_fRadius*m_fRadius);
Real bb4ac = b*b - 4*a*c;
Real mu1 = (-b + sqrt(bb4ac)) / (2 * a);
Real mu2 = (-b - sqrt(bb4ac)) / (2 * a);
Real mu = Min(mu1,mu2);
if( mu < 0 || mu > 1 ) mu = Max(mu1,mu2);
f_distance = mu * c_ray.GetLength();
return true;
}