hydra/src/triangle3.cpp

//
//  KRTriangle.cpp
//  Kraken Engine / Hydra
//
//  Copyright 2018 Kearwood Gilbert. All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without modification, are
//  permitted provided that the following conditions are met:
//
//  1. Redistributions of source code must retain the above copyright notice, this list of
//  conditions and the following disclaimer.
//
//  2. Redistributions in binary form must reproduce the above copyright notice, this list
//  of conditions and the following disclaimer in the documentation and/or other materials
//  provided with the distribution.
//
//  THIS SOFTWARE IS PROVIDED BY KEARWOOD GILBERT ''AS IS'' AND ANY EXPRESS OR IMPLIED
//  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
//  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL KEARWOOD GILBERT OR
//  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
//  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
//  ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
//  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//  The views and conclusions contained in the software and documentation are those of the
//  authors and should not be interpreted as representing official policies, either expressed
//  or implied, of Kearwood Gilbert.
//

#include "../include/hydra.h"

using namespace kraken;

namespace {
  bool _intersectSphere(const Vector3 &start, const Vector3 &dir, const Vector3 &sphere_center, float sphere_radius, float &distance)
  {
    // dir must be normalized

    // From: http://archive.gamedev.net/archive/reference/articles/article1026.html

    // TODO - Move to another class?
    Vector3 Q = sphere_center - start;
    float c = Q.magnitude();
    float v = Vector3::Dot(Q, dir);
    float d = sphere_radius * sphere_radius - (c * c - v * v);



    if (d < 0.0) {
      // No intersection
      return false;
    }

    // Return the distance to the [first] intersecting point

    distance = v - sqrt(d);
    if (distance < 0.0f) {
      return false;
    }
    return true;

  }

  bool _sameSide(const Vector3 &p1, const Vector3 &p2, const Vector3 &a, const Vector3 &b)
  {
    // TODO - Move to Vector3 class?
    // From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle

    Vector3 cp1 = Vector3::Cross(b - a, p1 - a);
    Vector3 cp2 = Vector3::Cross(b - a, p2 - a);
    if (Vector3::Dot(cp1, cp2) >= 0) return true;
    return false;
  }

  Vector3 _closestPointOnLine(const Vector3 &a, const Vector3 &b, const Vector3 &p)
  {
    // From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle

    // Determine t (the length of the vector from ‘a’ to ‘p’)

    Vector3 c = p - a;
    Vector3 V = Vector3::Normalize(b - a);
    float d = (a - b).magnitude();
    float t = Vector3::Dot(V, c);

    // Check to see if ‘t’ is beyond the extents of the line segment

    if (t < 0) return a;
    if (t > d) return b;

    // Return the point between ‘a’ and ‘b’

    return a + V * t;
  }
} // anonymous namespace

namespace kraken {

void Triangle3::init(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
{
    vert[0] = v1;
    vert[1] = v2;
    vert[2] = v3;
}

void Triangle3::init(const Triangle3 &tri)
{
    vert[0] = tri[0];
    vert[1] = tri[1];
    vert[2] = tri[2];
}


bool Triangle3::operator ==(const Triangle3& b) const
{
    return vert[0] == b[0] && vert[1] == b[1] && vert[2] == b[2];
}

bool Triangle3::operator !=(const Triangle3& b) const
{
    return vert[0] != b[0] || vert[1] != b[1] || vert[2] != b[2];
}

Vector3& Triangle3::operator[](unsigned int i)
{
    return vert[i];
}

Vector3 Triangle3::operator[](unsigned int i) const
{
    return vert[i];
}


bool Triangle3::rayCast(const Vector3 &start, const Vector3 &dir, Vector3 &hit_point) const
{
    // algorithm based on Dan Sunday's implementation at http://geomalgorithms.com/a06-_intersect-2.html
    const float SMALL_NUM = 0.00000001;     // anything that avoids division overflow
    Vector3   u, v, n;                    // triangle vectors
    Vector3   w0, w;                      // ray vectors
    float       r, a, b;                    // params to calc ray-plane intersect
    
    // get triangle edge vectors and plane normal
    u = vert[1] - vert[0];
    v = vert[2] - vert[0];
    n = Vector3::Cross(u, v); // cross product
    if (n == Vector3::Zero())             // triangle is degenerate
        return false;                  // do not deal with this case
    
    w0 = start - vert[0];
    a = -Vector3::Dot(n, w0);
    b = Vector3::Dot(n,dir);
    if (fabs(b) < SMALL_NUM) {     // ray is  parallel to triangle plane
        if (a == 0)
            return false; // ray lies in triangle plane
        else {
            return false; // ray disjoint from plane
        }
    }
    
    // get intersect point of ray with triangle plane
    r = a / b;
    if (r < 0.0)                    // ray goes away from triangle
        return false;                   // => no intersect
    // for a segment, also test if (r > 1.0) => no intersect
    
    
    Vector3 plane_hit_point = start + dir * r;            // intersect point of ray and plane
    
    // is plane_hit_point inside triangle?
    float    uu, uv, vv, wu, wv, D;
    uu = Vector3::Dot(u,u);
    uv = Vector3::Dot(u,v);
    vv = Vector3::Dot(v,v);
    w = plane_hit_point - vert[0];
    wu = Vector3::Dot(w,u);
    wv = Vector3::Dot(w,v);
    D = uv * uv - uu * vv;
    
    // get and test parametric coords
    float s, t;
    s = (uv * wv - vv * wu) / D;
    if (s < 0.0 || s > 1.0)         // plane_hit_point is outside triangle
        return false;
    t = (uv * wu - uu * wv) / D;
    if (t < 0.0 || (s + t) > 1.0)  // plane_hit_point is outside triangle
        return false;

    // plane_hit_point is inside the triangle
    hit_point = plane_hit_point;
    return true;
}

Vector3 Triangle3::calculateNormal() const
{
    Vector3 v1 = vert[1] - vert[0];
    Vector3 v2 = vert[2] - vert[0];
    
    return Vector3::Normalize(Vector3::Cross(v1, v2));
}

Vector3 Triangle3::closestPointOnTriangle(const Vector3 &p) const
{
    Vector3 a = vert[0];
    Vector3 b = vert[1];
    Vector3 c = vert[2];
    
    Vector3 Rab = _closestPointOnLine(a, b, p);
    Vector3 Rbc = _closestPointOnLine(b, c, p);
    Vector3 Rca = _closestPointOnLine(c, a, p);
    
    // return closest [Rab, Rbc, Rca] to p;
    float sd_Rab = (p - Rab).sqrMagnitude();
    float sd_Rbc = (p - Rbc).sqrMagnitude();
    float sd_Rca = (p - Rca).sqrMagnitude();
    
    if(sd_Rab < sd_Rbc && sd_Rab < sd_Rca) {
        return Rab;
    } else if(sd_Rbc < sd_Rab && sd_Rbc < sd_Rca) {
        return Rbc;
    } else {
        return Rca;
    }
}

bool Triangle3::sphereCast(const Vector3 &start, const Vector3 &dir, float radius, Vector3 &hit_point, float &hit_distance) const
{
    // Dir must be normalized
    const float SMALL_NUM = 0.001f;     // anything that avoids division overflow
    
    Vector3 tri_normal = calculateNormal();
    
    float d = Vector3::Dot(tri_normal, vert[0]);
    float e = Vector3::Dot(tri_normal, start) - radius;
    float cotangent_distance = e - d;
    
    Vector3 plane_intersect;
    float plane_intersect_distance;
    
    float denom = Vector3::Dot(tri_normal, dir);
    
    if(denom > -SMALL_NUM) {
        return false; // dir is co-planar with the triangle or going in the direction of the normal; no intersection
    }

    // Detect an embedded plane, caused by a sphere that is already intersecting the plane.
    if(cotangent_distance <= 0 && cotangent_distance >= -radius * 2.0f) {
        // Embedded plane - Sphere is already intersecting the plane.
        // Use the point closest to the origin of the sphere as the intersection
        plane_intersect = start - tri_normal * (cotangent_distance + radius);
        plane_intersect_distance = 0.0f;
    } else {
        // Sphere is not intersecting the plane
        // Determine the first point hit by the swept sphere on the triangle's plane
        
        plane_intersect_distance = -(cotangent_distance / denom);
        plane_intersect = start + dir * plane_intersect_distance - tri_normal * radius;
    }
    
    if(plane_intersect_distance < 0.0f) {
        return false;
    }
    
    if(containsPoint(plane_intersect)) {
        // Triangle contains point
        hit_point = plane_intersect;
        hit_distance = plane_intersect_distance;
        return true;
    } else {
        // Triangle does not contain point, cast ray back to sphere from closest point on triangle edge or vertice
        Vector3 closest_point = closestPointOnTriangle(plane_intersect);
        float reverse_hit_distance;
        if(_intersectSphere(closest_point, -dir, start, radius, reverse_hit_distance)) {
            // Reverse cast hit sphere
            hit_distance = reverse_hit_distance;
            hit_point = closest_point;
            return true;
        } else {
            // Reverse cast did not hit sphere
            return false;
        }
    }
}


bool Triangle3::containsPoint(const Vector3 &p) const
{
    /*
    // From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
    
    const float SMALL_NUM = 0.00000001f;     // anything that avoids division overflow
    // Vector3 A = vert[0], B = vert[1], C = vert[2];
    if (_sameSide(p, vert[0], vert[1], vert[2]) && _sameSide(p, vert[1], vert[0], vert[2]) && _sameSide(p, vert[2], vert[0], vert[1])) {
        Vector3 vc1 = Vector3::Cross(vert[0] - vert[1], vert[0] - vert[2]);
        if(fabs(Vector3::Dot(vert[0] - p, vc1)) <= SMALL_NUM) {
            return true;
        }
    }
    
    return false;
    */
    
    // From: http://blogs.msdn.com/b/rezanour/archive/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests.aspx
    
    Vector3 A = vert[0];
    Vector3 B = vert[1];
    Vector3 C = vert[2];
    Vector3 P = p;
    
    // Prepare our barycentric variables
    Vector3 u = B - A;
    Vector3 v = C - A;
    Vector3 w = P - A;
    
    Vector3 vCrossW = Vector3::Cross(v, w);
    Vector3 vCrossU = Vector3::Cross(v, u);
    
    // Test sign of r
    if (Vector3::Dot(vCrossW, vCrossU) < 0)
        return false;
    
    Vector3 uCrossW = Vector3::Cross(u, w);
    Vector3 uCrossV = Vector3::Cross(u, v);
    
    // Test sign of t
    if (Vector3::Dot(uCrossW, uCrossV) < 0)
        return false;
    
    // At this point, we know that r and t and both > 0.
    // Therefore, as long as their sum is <= 1, each must be less <= 1
    float denom = uCrossV.magnitude();
    float r = vCrossW.magnitude() / denom;
    float t = uCrossW.magnitude() / denom;
    
    return (r + t <= 1);
}

} // namespace kraken