328 lines
9.7 KiB
C++
Executable File
328 lines
9.7 KiB
C++
Executable File
//
|
||
// KRTriangle.cpp
|
||
// Kraken
|
||
//
|
||
// Created by Kearwood Gilbert on 2/8/2014.
|
||
// Copyright (c) 2014 Kearwood Software. All rights reserved.
|
||
//
|
||
|
||
#include "KRTriangle3.h"
|
||
#include "KRVector3.h"
|
||
|
||
KRTriangle3::KRTriangle3(const KRVector3 &v1, const KRVector3 &v2, const KRVector3 &v3)
|
||
{
|
||
m_c[0] = v1;
|
||
m_c[1] = v2;
|
||
m_c[2] = v3;
|
||
}
|
||
|
||
KRTriangle3::KRTriangle3(const KRTriangle3 &tri)
|
||
{
|
||
m_c[0] = tri[0];
|
||
m_c[1] = tri[1];
|
||
m_c[3] = tri[3];
|
||
}
|
||
|
||
|
||
KRTriangle3::~KRTriangle3()
|
||
{
|
||
|
||
}
|
||
|
||
bool KRTriangle3::operator ==(const KRTriangle3& b) const
|
||
{
|
||
return m_c[0] == b[0] && m_c[1] == b[1] && m_c[2] == b[2];
|
||
}
|
||
|
||
bool KRTriangle3::operator !=(const KRTriangle3& b) const
|
||
{
|
||
return m_c[0] != b[0] || m_c[1] != b[1] || m_c[2] != b[2];
|
||
}
|
||
|
||
KRTriangle3& KRTriangle3::operator =(const KRTriangle3& b)
|
||
{
|
||
|
||
m_c[0] = b[0];
|
||
m_c[1] = b[1];
|
||
m_c[3] = b[3];
|
||
return *this;
|
||
}
|
||
|
||
KRVector3& KRTriangle3::operator[](unsigned i)
|
||
{
|
||
return m_c[i];
|
||
}
|
||
|
||
KRVector3 KRTriangle3::operator[](unsigned i) const
|
||
{
|
||
return m_c[i];
|
||
}
|
||
|
||
|
||
bool KRTriangle3::rayCast(const KRVector3 &start, const KRVector3 &dir, KRVector3 &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
|
||
KRVector3 u, v, n; // triangle vectors
|
||
KRVector3 w0, w; // ray vectors
|
||
float r, a, b; // params to calc ray-plane intersect
|
||
|
||
// get triangle edge vectors and plane normal
|
||
u = m_c[1] - m_c[0];
|
||
v = m_c[2] - m_c[0];
|
||
n = KRVector3::Cross(u, v); // cross product
|
||
if (n == KRVector3::Zero()) // triangle is degenerate
|
||
return false; // do not deal with this case
|
||
|
||
w0 = start - m_c[0];
|
||
a = -KRVector3::Dot(n, w0);
|
||
b = KRVector3::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
|
||
|
||
|
||
KRVector3 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 = KRVector3::Dot(u,u);
|
||
uv = KRVector3::Dot(u,v);
|
||
vv = KRVector3::Dot(v,v);
|
||
w = plane_hit_point - m_c[0];
|
||
wu = KRVector3::Dot(w,u);
|
||
wv = KRVector3::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;
|
||
}
|
||
|
||
KRVector3 KRTriangle3::calculateNormal() const
|
||
{
|
||
KRVector3 v1 = m_c[1] - m_c[0];
|
||
KRVector3 v2 = m_c[2] - m_c[0];
|
||
|
||
return KRVector3::Normalize(KRVector3::Cross(v1, v2));
|
||
}
|
||
|
||
bool _intersectSphere(const KRVector3 &start, const KRVector3 &dir, const KRVector3 &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?
|
||
KRVector3 Q = sphere_center - start;
|
||
float c = Q.magnitude();
|
||
float v = KRVector3::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 KRVector3 &p1, const KRVector3 &p2, const KRVector3 &a, const KRVector3 &b)
|
||
{
|
||
// TODO - Move to KRVector3 class?
|
||
// From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
|
||
|
||
KRVector3 cp1 = KRVector3::Cross(b - a, p1 - a);
|
||
KRVector3 cp2 = KRVector3::Cross(b - a, p2 - a);
|
||
if(KRVector3::Dot(cp1, cp2) >= 0) return true;
|
||
return false;
|
||
}
|
||
|
||
KRVector3 _closestPointOnLine(const KRVector3 &a, const KRVector3 &b, const KRVector3 &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’)
|
||
|
||
KRVector3 c = p - a;
|
||
KRVector3 V = KRVector3::Normalize(b - a);
|
||
float d = (a - b).magnitude();
|
||
float t = KRVector3::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;
|
||
}
|
||
|
||
KRVector3 KRTriangle3::closestPointOnTriangle(const KRVector3 &p) const
|
||
{
|
||
KRVector3 a = m_c[0];
|
||
KRVector3 b = m_c[1];
|
||
KRVector3 c = m_c[2];
|
||
|
||
KRVector3 Rab = _closestPointOnLine(a, b, p);
|
||
KRVector3 Rbc = _closestPointOnLine(b, c, p);
|
||
KRVector3 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 KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float radius, KRVector3 &hit_point, float &hit_distance) const
|
||
{
|
||
// Dir must be normalized
|
||
const float SMALL_NUM = 0.001f; // anything that avoids division overflow
|
||
|
||
KRVector3 tri_normal = calculateNormal();
|
||
|
||
float d = KRVector3::Dot(tri_normal, m_c[0]);
|
||
float e = KRVector3::Dot(tri_normal, start) - radius;
|
||
float cotangent_distance = e - d;
|
||
|
||
KRVector3 plane_intersect;
|
||
float plane_intersect_distance;
|
||
|
||
float denom = KRVector3::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
|
||
KRVector3 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 KRTriangle3::containsPoint(const KRVector3 &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
|
||
// KRVector3 A = m_c[0], B = m_c[1], C = m_c[2];
|
||
if (_sameSide(p, m_c[0], m_c[1], m_c[2]) && _sameSide(p, m_c[1], m_c[0], m_c[2]) && _sameSide(p, m_c[2], m_c[0], m_c[1])) {
|
||
KRVector3 vc1 = KRVector3::Cross(m_c[0] - m_c[1], m_c[0] - m_c[2]);
|
||
if(fabs(KRVector3::Dot(m_c[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
|
||
|
||
KRVector3 A = m_c[0];
|
||
KRVector3 B = m_c[1];
|
||
KRVector3 C = m_c[2];
|
||
KRVector3 P = p;
|
||
|
||
// Prepare our barycentric variables
|
||
KRVector3 u = B - A;
|
||
KRVector3 v = C - A;
|
||
KRVector3 w = P - A;
|
||
|
||
KRVector3 vCrossW = KRVector3::Cross(v, w);
|
||
KRVector3 vCrossU = KRVector3::Cross(v, u);
|
||
|
||
// Test sign of r
|
||
if (KRVector3::Dot(vCrossW, vCrossU) < 0)
|
||
return false;
|
||
|
||
KRVector3 uCrossW = KRVector3::Cross(u, w);
|
||
KRVector3 uCrossV = KRVector3::Cross(u, v);
|
||
|
||
// Test sign of t
|
||
if (KRVector3::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);
|
||
}
|
||
|
||
|
||
|