Additions for the boundingvolumes
Modified Files: SGGeometryTest.cxx SGIntersect.hxx SGLineSegment.hxx SGTriangle.hxx
This commit is contained in:
frohlich
@@ -31,6 +31,102 @@ SGVec3<T> rndVec3(void)
|
||||
return SGVec3<T>(sg_random(), sg_random(), sg_random());
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool
|
||||
TriangleClosestPointIntersectionTest(void)
|
||||
{
|
||||
unsigned nTests = 100000;
|
||||
unsigned failedCount = 0;
|
||||
for (unsigned i = 0; i < nTests; ++i) {
|
||||
// test triangle
|
||||
SGTriangle<T> triangle(rndVec3<T>(), rndVec3<T>(), rndVec3<T>());
|
||||
T triangleEps = 100*SGLimits<T>::epsilon();
|
||||
|
||||
// generate random point in the triangle
|
||||
T u = 4*sg_random() - 2;
|
||||
T v = 4*sg_random() - 2;
|
||||
if (1 < u + v) {
|
||||
v = T(0.5)*(v + 1 - u);
|
||||
u = 1 - v;
|
||||
}
|
||||
u = SGMisc<T>::clip(u, 0, 1);
|
||||
v = SGMisc<T>::clip(v, 0, 1);
|
||||
|
||||
SGVec3<T> testClosest;
|
||||
testClosest = triangle.getBaseVertex();
|
||||
testClosest += u*triangle.getEdge(0) + v*triangle.getEdge(1);
|
||||
|
||||
SGVec3<T> n = triangle.getNormal();
|
||||
SGVec3<T> e0 = triangle.getEdge(0);
|
||||
SGVec3<T> e1 = triangle.getEdge(1);
|
||||
|
||||
// The normals to the edges
|
||||
SGVec3<T> a0 = cross(e0, n);
|
||||
SGVec3<T> a1 = cross(e1 - e0, n);
|
||||
SGVec3<T> a2 = cross(n, e1);
|
||||
|
||||
SGVec3<T> testPoint = testClosest;
|
||||
// Ok, if we are at some edge, go perpandicular to the triangle away
|
||||
if (u == 0) {
|
||||
if (v == 0) {
|
||||
testPoint += sg_random()*a0 + sg_random()*a2;
|
||||
} else if (v == 1) {
|
||||
testPoint += sg_random()*a1 + sg_random()*a2;
|
||||
} else {
|
||||
testPoint += sg_random()*a2;
|
||||
}
|
||||
} else if (u == 1) {
|
||||
testPoint += sg_random()*a0 + sg_random()*a1;
|
||||
} else {
|
||||
if (v == 0) {
|
||||
testPoint += sg_random()*a0;
|
||||
} else if (u + v == 1) {
|
||||
testPoint += sg_random()*a1;
|
||||
}
|
||||
}
|
||||
testPoint += (2*sg_random() - 1)*n;
|
||||
|
||||
// Test the closest point function
|
||||
SGVec3<T> closest = closestPoint(triangle, testPoint);
|
||||
if (!equivalent(closest, testClosest, triangleEps)) {
|
||||
std::cout << "Failed closest point test #" << i
|
||||
<< ": not equivalent!\nu = "
|
||||
<< u << ", v = " << v
|
||||
<< "\ntestPoint = " << testPoint
|
||||
<< ", testClosest = " << testClosest
|
||||
<< ", closest = " << closest << "\n"
|
||||
<< triangle << std::endl;
|
||||
++failedCount;
|
||||
}
|
||||
|
||||
// Test the triangle sphere intersection
|
||||
SGSphere<T> sphere(testPoint, sg_random());
|
||||
bool exactIntersection = intersects(sphere, testClosest);
|
||||
// bool exactIntersection = intersects(sphere, closest);
|
||||
bool sphereTriangleIntersection = intersects(sphere, triangle);
|
||||
|
||||
if (sphereTriangleIntersection != exactIntersection) {
|
||||
std::cout << "Failed triangle sphere intersection test #" << i
|
||||
<< ": not equivalent!\nu = "
|
||||
<< u << ", v = " << v
|
||||
<< "\ntestPoint = " << testPoint
|
||||
<< ", testClosest = " << testClosest
|
||||
<< ", closest = " << closest << "\n"
|
||||
<< triangle << std::endl;
|
||||
++failedCount;
|
||||
}
|
||||
}
|
||||
|
||||
if (nTests < 100*failedCount) {
|
||||
std::cout << "Failed box line intersection tests: " << failedCount
|
||||
<< " tests out of " << nTests
|
||||
<< " went wrong. Abort!" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool
|
||||
TriangleLineIntersectionTest(void)
|
||||
@@ -426,6 +522,11 @@ main(void)
|
||||
|
||||
sg_srandom(17);
|
||||
|
||||
if (!TriangleClosestPointIntersectionTest<float>())
|
||||
return EXIT_FAILURE;
|
||||
if (!TriangleClosestPointIntersectionTest<double>())
|
||||
return EXIT_FAILURE;
|
||||
|
||||
if (!TriangleLineIntersectionTest<float>())
|
||||
return EXIT_FAILURE;
|
||||
if (!TriangleLineIntersectionTest<double>())
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
// Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
|
||||
// Copyright (C) 2006-2009 Mathias Froehlich - Mathias.Froehlich@web.de
|
||||
//
|
||||
// This library is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Library General Public
|
||||
@@ -550,6 +550,416 @@ intersects(const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline SGVec3<T>
|
||||
closestPoint(const SGTriangle<T>& tri, const SGVec3<T>& p)
|
||||
{
|
||||
// This method minimizes the distance function Q(u, v) = || p - x ||
|
||||
// where x is a point in the trialgle given by the vertices v_i
|
||||
// x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
|
||||
// The theoretical analysis is somehow too long for a comment.
|
||||
// May be it is sufficient to see that this code passes all the tests.
|
||||
|
||||
SGVec3<T> off = tri.getBaseVertex() - p;
|
||||
T a = dot(tri.getEdge(0), tri.getEdge(0));
|
||||
T b = dot(tri.getEdge(0), tri.getEdge(1));
|
||||
T c = dot(tri.getEdge(1), tri.getEdge(1));
|
||||
T d = dot(tri.getEdge(0), off);
|
||||
T e = dot(tri.getEdge(1), off);
|
||||
|
||||
T det = a*c - b*b;
|
||||
|
||||
T u = b*e - c*d;
|
||||
T v = b*d - a*e;
|
||||
|
||||
// Regions
|
||||
// \2|
|
||||
// \|
|
||||
// |\
|
||||
// 3 |0\ 1
|
||||
//----------
|
||||
// 4 | 5 \ 6
|
||||
|
||||
if (u + v <= det) {
|
||||
if (u < 0) {
|
||||
if (v < 0) {
|
||||
// region 4
|
||||
if (d < 0) {
|
||||
if (a <= -d) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + tri.getEdge(0);
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0);
|
||||
}
|
||||
} else {
|
||||
if (0 < e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else if (c <= -e) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
return tri.getBaseVertex() + tri.getEdge(1);
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
return tri.getBaseVertex() + v*tri.getEdge(1);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// region 3
|
||||
if (0 <= e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else if (c <= -e) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
return tri.getBaseVertex() + tri.getEdge(1);
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
return tri.getBaseVertex() + v*tri.getEdge(1);
|
||||
}
|
||||
}
|
||||
} else if (v < 0) {
|
||||
// region 5
|
||||
if (0 <= d) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else if (a <= -d) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + tri.getEdge(0);
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0);
|
||||
}
|
||||
} else {
|
||||
// region 0
|
||||
if (det <= SGLimits<T>::min()) {
|
||||
u = 0;
|
||||
v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else {
|
||||
T invDet = 1/det;
|
||||
u *= invDet;
|
||||
v *= invDet;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (u < 0) {
|
||||
// region 2
|
||||
T tmp0 = b + d;
|
||||
T tmp1 = c + e;
|
||||
if (tmp0 < tmp1) {
|
||||
T numer = tmp1 - tmp0;
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + tri.getEdge(0);
|
||||
} else {
|
||||
u = numer/denom;
|
||||
v = 1 - u;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
|
||||
}
|
||||
} else {
|
||||
if (tmp1 <= 0) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
return tri.getBaseVertex() + tri.getEdge(1);
|
||||
} else if (0 <= e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
return tri.getBaseVertex() + v*tri.getEdge(1);
|
||||
}
|
||||
}
|
||||
} else if (v < 0) {
|
||||
// region 6
|
||||
T tmp0 = b + e;
|
||||
T tmp1 = a + d;
|
||||
if (tmp0 < tmp1) {
|
||||
T numer = tmp1 - tmp0;
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
return tri.getBaseVertex() + tri.getEdge(1);
|
||||
} else {
|
||||
v = numer/denom;
|
||||
u = 1 - v;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
|
||||
}
|
||||
} else {
|
||||
if (tmp1 < 0) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + tri.getEdge(0);
|
||||
} else if (0 <= d) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex();
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// region 1
|
||||
T numer = c + e - b - d;
|
||||
if (numer <= 0) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
return tri.getVertex(2);
|
||||
} else {
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
return tri.getBaseVertex() + tri.getEdge(0);
|
||||
} else {
|
||||
u = numer/denom;
|
||||
v = 1 - u;
|
||||
return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
template<typename T>
|
||||
inline SGVec3<T>
|
||||
closestPoint(const SGVec3<T>& p, const SGTriangle<T>& tri)
|
||||
{ return closestPoint(tri, p); }
|
||||
|
||||
template<typename T>
|
||||
inline bool
|
||||
intersects(const SGTriangle<T>& tri, const SGSphere<T>& sphere)
|
||||
{
|
||||
// This method minimizes the distance function Q(u, v) = || p - x ||
|
||||
// where x is a point in the trialgle given by the vertices v_i
|
||||
// x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
|
||||
// The theoretical analysis is somehow too long for a comment.
|
||||
// May be it is sufficient to see that this code passes all the tests.
|
||||
|
||||
SGVec3<T> off = tri.getBaseVertex() - sphere.getCenter();
|
||||
T baseDist2 = dot(off, off);
|
||||
T a = dot(tri.getEdge(0), tri.getEdge(0));
|
||||
T b = dot(tri.getEdge(0), tri.getEdge(1));
|
||||
T c = dot(tri.getEdge(1), tri.getEdge(1));
|
||||
T d = dot(tri.getEdge(0), off);
|
||||
T e = dot(tri.getEdge(1), off);
|
||||
|
||||
T det = a*c - b*b;
|
||||
|
||||
T u = b*e - c*d;
|
||||
T v = b*d - a*e;
|
||||
|
||||
// Regions
|
||||
// \2|
|
||||
// \|
|
||||
// |\
|
||||
// 3 |0\ 1
|
||||
//----------
|
||||
// 4 | 5 \ 6
|
||||
|
||||
if (u + v <= det) {
|
||||
if (u < 0) {
|
||||
if (v < 0) {
|
||||
// region 4
|
||||
if (d < 0) {
|
||||
if (a <= -d) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
T sqrDist = a + 2*d + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
T sqrDist = d*u + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
} else {
|
||||
if (0 < e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else if (c <= -e) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
T sqrDist = c + 2*e + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
T sqrDist = e*v + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// region 3
|
||||
if (0 <= e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else if (c <= -e) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
T sqrDist = c + 2*e + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
T sqrDist = e*v + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
} else if (v < 0) {
|
||||
// region 5
|
||||
if (0 <= d) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else if (a <= -d) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
T sqrDist = a + 2*d + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
T sqrDist = d*u + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
} else {
|
||||
// region 0
|
||||
if (det <= SGLimits<T>::min()) {
|
||||
// sqrDist = baseDist2;
|
||||
u = 0;
|
||||
v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else {
|
||||
T invDet = 1/det;
|
||||
u *= invDet;
|
||||
v *= invDet;
|
||||
T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (u < 0) {
|
||||
// region 2
|
||||
T tmp0 = b + d;
|
||||
T tmp1 = c + e;
|
||||
if (tmp0 < tmp1) {
|
||||
T numer = tmp1 - tmp0;
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
T sqrDist = a + 2*d + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
u = numer/denom;
|
||||
v = 1 - u;
|
||||
T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
} else {
|
||||
if (tmp1 <= 0) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
T sqrDist = c + 2*e + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else if (0 <= e) {
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else {
|
||||
// u = 0;
|
||||
v = -e/c;
|
||||
T sqrDist = e*v + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
} else if (v < 0) {
|
||||
// region 6
|
||||
T tmp0 = b + e;
|
||||
T tmp1 = a + d;
|
||||
if (tmp0 < tmp1) {
|
||||
T numer = tmp1 - tmp0;
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
T sqrDist = c + 2*e + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
v = numer/denom;
|
||||
u = 1 - v;
|
||||
T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e)+baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
} else {
|
||||
if (tmp1 < 0) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
T sqrDist = a + 2*d + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else if (0 <= d) {
|
||||
// sqrDist = baseDist2;
|
||||
// u = 0;
|
||||
// v = 0;
|
||||
return baseDist2 <= sphere.getRadius2();
|
||||
} else {
|
||||
u = -d/a;
|
||||
// v = 0;
|
||||
T sqrDist = d*u + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// region 1
|
||||
T numer = c + e - b - d;
|
||||
if (numer <= 0) {
|
||||
// u = 0;
|
||||
// v = 1;
|
||||
T sqrDist = c + 2*e + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
T denom = a - 2*b + c;
|
||||
if (denom <= numer) {
|
||||
// u = 1;
|
||||
// v = 0;
|
||||
T sqrDist = a + 2*d + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
} else {
|
||||
u = numer/denom;
|
||||
v = 1 - u;
|
||||
T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
|
||||
return sqrDist <= sphere.getRadius2();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
template<typename T>
|
||||
inline bool
|
||||
intersects(const SGSphere<T>& sphere, const SGTriangle<T>& tri)
|
||||
{ return intersects(tri, sphere); }
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline bool
|
||||
intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
|
||||
|
||||
@@ -48,6 +48,14 @@ public:
|
||||
SGVec3<T> getCenter() const
|
||||
{ return _start + T(0.5)*_direction; }
|
||||
|
||||
SGLineSegment<T> transform(const SGMatrix<T>& matrix) const
|
||||
{
|
||||
SGLineSegment<T> lineSegment;
|
||||
lineSegment._start = matrix.xformPt(_start);
|
||||
lineSegment._direction = matrix.xformVec(_direction);
|
||||
return lineSegment;
|
||||
}
|
||||
|
||||
private:
|
||||
SGVec3<T> _start;
|
||||
SGVec3<T> _direction;
|
||||
|
||||
@@ -71,6 +71,16 @@ public:
|
||||
_d[0] = _d[1];
|
||||
_d[1] = tmp;
|
||||
}
|
||||
|
||||
SGTriangle<T> transform(const SGMatrix<T>& matrix) const
|
||||
{
|
||||
SGTriangle<T> triangle;
|
||||
triangle._v0 = matrix.xformPt(_v0);
|
||||
triangle._d[0] = matrix.xformVec(_d[0]);
|
||||
triangle._d[1] = matrix.xformVec(_d[1]);
|
||||
return triangle;
|
||||
}
|
||||
|
||||
private:
|
||||
/// Store one vertex directly, _d is the offset of the other two
|
||||
/// vertices wrt the base vertex
|
||||
|
||||
Reference in New Issue
Block a user