source: GTP/trunk/Lib/Vis/Preprocessing/src/Triangle3.cpp @ 2575

#include "Triangle3.h"
#include "Ray.h"
#include "SimpleRay.h"
#include "AxisAlignedBox3.h"
#include "Containers.h"
#include "Polygon3.h"


using namespace std;

namespace GtpVisibilityPreprocessor {

        
Triangle3::Triangle3(const Vector3 &a, const Vector3 &b, const Vector3 &c) 
{
        Init(a, b, c);
}


void Triangle3::Init(const Vector3 &a, const Vector3 &b, const Vector3 &c) 
{
        mVertices[0] = a;
        mVertices[1] = b;
        mVertices[2] = c;
}


float Triangle3::GetSpatialAngle(const Vector3 &point) const
{
        return 0.0f;
}


int
Triangle3::CastRay(const Ray &ray,
                   float &t,
                   const float nearestT,
                   Vector3 &normal) const
{
  //////////////
  // specialised triangle ray casting version
  // using ray-plane intersection
  
  // get triangle edge vectors and plane normal
  const Vector3 u = mVertices[0] - mVertices[1];
  const Vector3 v = mVertices[2] - mVertices[1];
  
  // cross product
  normal = Normalize(CrossProd(v, u));
  
  // ray direction vector
  const Vector3 dir = ray.GetDir();
  const Vector3 w0 = ray.GetLoc() - mVertices[1];
  
  // params to calc ray-plane intersect
  const float a = -DotProd(normal, w0);
  const float b = DotProd(normal, dir);
  
  // check for division by zero
  if (fabs(b) < Limits::Small) 
    {   
      // ray is parallel to triangle plane
      if (a == 0)
        {
          // ray lies in triangle plane
          return Ray::INTERSECTION_OUT_OF_LIMITS;
        }
      else
        {
          // ray disjoint from plane
          return Ray::NO_INTERSECTION;
        }
    }

  // distance from origin of ray to plane
  t = a / b;
  
  if (t <= Limits::Small) // ray goes away from triangle
    {
      return Ray::INTERSECTION_OUT_OF_LIMITS;
    }
  // already found nearer intersection
  else if ((ray.GetType() == Ray::LOCAL_RAY) && (t >= nearestT))
    {
      return Ray::NO_INTERSECTION;
    }
  
  /////////////////
    //-- found intersection point
  //-- check if it is inside triangle
  
  const Vector3 pt = ray.GetLoc() + t * dir; 
#if GTP_DEBUG
  if (!pt.CheckValidity())
    {
      cout << "tr: " << *this << endl;
      cout << "v: " << pt << " t: " << t << " a: " << a << " b: " << b << " n: " << normal << endl;
    }
#endif
  const Vector3 w = pt - mVertices[1];
  
  const float uu = DotProd(u, u);
  const float uv = DotProd(u, v);
  const float vv = DotProd(v, v);
  
  const float wu = DotProd(w, u);
  const float wv = DotProd(w, v);
  
  
  const float D = uv * uv - uu * vv;
  
  // get and test parametric coords
  const float s = (uv * wv - vv * wu) / D;
  
  if ((s < -Limits::Small) || (s > 1.0f + Limits::Small)) // pt is outside triangle
    {   
      return Ray::NO_INTERSECTION;
    }
  
  const float s2 = (uv * wu - uu * wv) / D;
  
  if ((s2 < -Limits::Small) || ((s + s2) > 1.0f + Limits::Small)) // pt is outside triangle
    {   
      return Ray::NO_INTERSECTION;
    }
  
  return Ray::INTERSECTION; // I is in T
}


int
Triangle3::CastSimpleRay(const SimpleRay &ray,
                         float &t,
                         const float nearestT) const
{
  //////////////
  // specialised triangle ray casting version
  // using ray-plane intersection
  
  // get triangle edge vectors and plane normal
  const Vector3 u = mVertices[0] - mVertices[1];
  const Vector3 v = mVertices[2] - mVertices[1];

  // cross product
  const Vector3 normal = Normalize(CrossProd(v, u));

  // ray direction vector
  const Vector3 dir = ray.mDirection;
  const Vector3 w0 = ray.mOrigin - mVertices[1];

  // params to calc ray-plane intersect
  const float a = -DotProd(normal, w0);
  const float b = DotProd(normal, dir);

  // check for division by zero
  if (fabs(b) < Limits::Small) 
  {   
    // ray is parallel to triangle plane
    if (a == 0)
    {
      // ray lies in triangle plane
      return Ray::INTERSECTION_OUT_OF_LIMITS;
    }
    else {
      // ray disjoint from plane
      return Ray::NO_INTERSECTION;
    }
  }

  // distance from origin of ray to plane
  t = a / b;

  if (t <= Limits::Small) // ray goes away from triangle
  {
    return Ray::INTERSECTION_OUT_OF_LIMITS;
  }

  // already found nearer intersection
  if (t > nearestT + 1e-5)
  {
    return Ray::NO_INTERSECTION;
  }

  /////////////////
  //-- found intersection point
  //-- check if it is inside triangle
  
  const Vector3 pt = ray.mOrigin + t * dir; 
#if GTP_DEBUG
  if (!pt.CheckValidity())
  {
    cout << "tr: " << *this << endl;
    cout << "v: " << pt << " t: " << t << " a: " << a << " b: " << b << " n: " << normal << endl;
  }
#endif
  const Vector3 w = pt - mVertices[1];

  const float uu = DotProd(u, u);
  const float uv = DotProd(u, v);
  const float vv = DotProd(v, v);
    
  const float wu = DotProd(w, u);
  const float wv = DotProd(w, v);
  
  
  const float D = uv * uv - uu * vv;
  
  // get and test parametric coords
  const float s = (uv * wv - vv * wu) / D;
  
  if ((s < -Limits::Small) ||
      (s > 1.0f + Limits::Small)) // pt is outside triangle
  {     
    return Ray::NO_INTERSECTION;
  }

  const float s2 = (uv * wu - uu * wv) / D;
  
  if ((s2 < -Limits::Small) ||
      ((s + s2) > 1.0f + Limits::Small)) // pt is outside triangle
  {     
    return Ray::NO_INTERSECTION;
  }
  
  return Ray::INTERSECTION; // I is in T
}
  

AxisAlignedBox3 Triangle3::GetBoundingBox() const
{
        AxisAlignedBox3 box;
        box.Initialize();

        box.Include(mVertices[0]);
        box.Include(mVertices[1]);
        box.Include(mVertices[2]);

        box.EnlargeToMinSize();
        
        return box;
}


Vector3 Triangle3::GetNormal() const 
{
        const Vector3 v1 = mVertices[0] - mVertices[1];
        const Vector3 v2 = mVertices[2] - mVertices[1];

        return Normalize(CrossProd(v2, v1));
}


Vector3 Triangle3::GetCenter() const 
{
        return (mVertices[0] + mVertices[1] + mVertices[2]) / 3.0f;
}


float Triangle3::GetArea() const 
{
        Vector3 v1 = mVertices[0] - mVertices[1], v2=mVertices[2] - mVertices[1];
        return 0.5f * Magnitude(CrossProd(v2, v1));
}


bool Triangle3::CheckValidity() const
{
        return !(
                EpsilonEqualV3(mVertices[0], mVertices[1]) ||
                EpsilonEqualV3(mVertices[0], mVertices[2]) ||
                EpsilonEqualV3(mVertices[1], mVertices[2])
                );
}


bool Triangle3::GetPlaneIntersection(const Plane3 &plane, 
                                                                         Vector3 &intersectA, 
                                                                         Vector3 &intersectB) const
{
        int side[3];

        // compute distance from plane
        for (int i = 0; i < 3; ++ i)
        {
                side[i] = plane.Side(mVertices[i], Limits::Small);
        }

        /////
        // no intersection => early exit
        if (((side[0] > 0) && (side[1] > 0) && (side[2] > 0)) ||
                ((side[0] < 0) && (side[1] < 0) && (side[2] < 0)))
        {
                return false;
        }

        /////////////
        // at least 2 triangle vertices lie in plane => early exit

        for (int i = 0; i < 3; ++ i)
        {
                if (!side[i] && !side[(i + 1) % 3])
                {
                        intersectA = mVertices[i];
                        intersectB = mVertices[(i + 1) % 3];
                        
                        return true;
                }
        }

        bool foundA = false;

        // compute intersection points
        for (int i = 0; i < 3; ++ i)
        {
                const int i_2 = (i + 1) % 3;

                // intersection found
                if ((side[i] >= 0) && (side[i_2] <= 0) ||
                        (side[i] <= 0) && (side[i_2] >= 0))
                {       
                        const float t = plane.FindT(mVertices[i], mVertices[i_2]);
                        
                        if (!foundA)
                        {
                                intersectA = mVertices[i] + t * (mVertices[i_2] - mVertices[i]);
                        
                                foundA = true;
                        }
                        else
                        {
                                intersectB = mVertices[i] + t * (mVertices[i_2] - mVertices[i]);
                        
                                return true;
                        }
                }
        }

        cout << "warning! wrong triangle - plane intersection" << endl;
        return false; // something went wrong!
}


}