clipping.hpp

Go to the documentation of this file.

#ifndef COB_3D_CLIPPING_HPP
#define COB_3D_CLIPPING_HPP

namespace cob_3d_mapping
{
  bool CohenSutherLandClipping::clip(const Eigen::Vector3f& p0,
                                     const Eigen::Vector3f& p1,
                                     Eigen::Vector3f& q0,
                                     Eigen::Vector3f& q1)
  {
    int code0 = computeCode(p0(0),p0(1),p0(2));
    int code1 = computeCode(p1(0),p1(1),p1(2));
    bool accept = false;

    if( !(code0 | code1) ) // trivial accept
    {
      accept = true;
      q0 = p0;
      q1 = p1;
    }
    else if( !(code0 & code1) )
    {
      /* Line:  q = (p1 - p0) * d + p0
       * Plane: (q - c).dot(n)           c: point on plane, n: normal of plane
       *
       *           (p0 - c).dot(n)
       * => d =  ------------------
       *          (p1 - p0).dot(n)
       */
      float x, y, z, d;
      int outcode;
      Eigen::Vector3f *pnew;
      const Eigen::Vector3f *pold;
      if(code0)
      { // check which point lies outside
        outcode = code0;
        pnew = &q0;
        pold = &p1;
      }
      else
      {
        outcode = code1;
        pold = &p0;
        pnew = &q1;
      }

      if(outcode & FAR)
      {
        d = (zmax - p0(2)) / (p1(2) - p0(2));
        (*pnew)(0) = (p1(0) - p0(0)) * d + p0(0);
        (*pnew)(1) = (p1(1) - p0(1)) * d + p0(1);
        (*pnew)(2) = zmax;
      }
      else if(outcode & NEAR)
      {
        d = (zmin - p0(2)) / (p1(2) - p0(2));
        (*pnew)(0) = (p1(0) - p0(0)) * d + p0(0);
        (*pnew)(1) = (p1(1) - p0(1)) * d + p0(1);
        (*pnew)(2) = zmin;
      }
      else if(outcode & BOTTOM)
      {
        d = (ymax - p0(1)) / (p1(1) - p0(1));
        (*pnew)(0) = (p1(0) - p0(0)) * d + p0(0);
        (*pnew)(1) = ymax;
        (*pnew)(2) = (p1(2) - p0(2)) * d + p0(2);
      }
      else if(outcode & TOP)
      {
        d = (ymin - p0(1)) / (p1(1) - p0(1));
        (*pnew)(0) = (p1(0) - p0(0)) * d + p0(0);
        (*pnew)(1) = ymin;
        (*pnew)(2) = (p1(2) - p0(2)) * d + p0(2);
      }
      else if(outcode & RIGHT)
      {
        d = (xmax - p0(0)) / (p1(0) - p0(0));
        (*pnew)(0) = xmax;
        (*pnew)(1) = (p1(1) - p0(1)) * d + p0(1);
        (*pnew)(2) = (p1(2) - p0(2)) * d + p0(2);
      }
      else /*(outcode & LEFT)*/
      {
        d = (xmin - p0(0)) / (p1(0) - p0(0));
        (*pnew)(0) = xmin;
        (*pnew)(1) = (p1(1) - p0(1)) * d + p0(1);
        (*pnew)(2) = (p1(2) - p0(2)) * d + p0(2);
      }
      accept = clip(*pold,*pnew,q0,q1);
    }

    return accept;
  }
}