ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/ray3.cpp
Go to the documentation of this file.
00001 
00007 #include "ray3.h"
00008 #include "plane.h"
00009 
00010 namespace argos {
00011 
00012    /****************************************/
00013    /****************************************/
00014 
00015    bool CRay3::Intersects(const CPlane& c_plane,
00016                           CVector3& c_point) const {
00017       CVector3 cRayVec;
00018       Real fDen = c_plane.GetNormal().DotProduct(ToVector(cRayVec));
00019       if(Abs(fDen) > 0.0f) {
00020          /* There could be intersection */
00021          Real fTOnRay = -c_plane.GetNormal().DotProduct(m_cStart - c_plane.GetPosition()) / fDen;
00022          if(fTOnRay < 0.0f || fTOnRay > 1.0f) {
00023             /* Intersection point is beyond ray extrema */
00024             return false;
00025          }
00026          else {
00027             /* There is an intersection */
00028             GetPoint(c_point, fTOnRay);
00029             return true;
00030          }
00031       }
00032       else {
00033          /* No intersection */
00034          return false;
00035       }
00036    }
00037    
00038    /****************************************/
00039    /****************************************/
00040 
00041 }