-// 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
template<typename T>
inline bool
-intersects(const SGBox<T>& box, const SGSphere<T>& sphere)
+intersects(const SGSphere<T>& s1, const SGSphere<T>& s2)
+{
+ if (s1.empty())
+ return false;
+ if (s2.empty())
+ return false;
+
+ T dist = s1.getRadius() + s2.getRadius();
+ return distSqr(s1.getCenter(), s2.getCenter()) <= dist*dist;
+}
+
+
+template<typename T1, typename T2>
+inline bool
+intersects(const SGBox<T1>& box, const SGSphere<T2>& sphere)
{
if (sphere.empty())
return false;
return true;
}
// make it symmetric
-template<typename T>
+template<typename T1, typename T2>
inline bool
-intersects(const SGSphere<T>& sphere, const SGBox<T>& box)
+intersects(const SGSphere<T1>& sphere, const SGBox<T2>& box)
{ return intersects(box, sphere); }
-template<typename T>
+template<typename T1, typename T2>
inline bool
-intersects(const SGVec3<T>& v, const SGBox<T>& box)
+intersects(const SGVec3<T1>& v, const SGBox<T2>& box)
{
if (v[0] < box.getMin()[0])
return false;
return false;
return true;
}
-template<typename T>
+template<typename T1, typename T2>
inline bool
-intersects(const SGBox<T>& box, const SGVec3<T>& v)
+intersects(const SGBox<T1>& box, const SGVec3<T2>& v)
{ return intersects(v, box); }
inline bool
intersects(const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
{
- // FIXME: for now just wrap the othr method. When that has prooven
+ // FIXME: for now just wrap the other method. When that has prooven
// well optimized, implement that special case
SGVec3<T> dummy;
return intersects(dummy, tri, lineSegment, 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, typename T2>
+inline bool
+intersects(const SGTriangle<T>& tri, const SGSphere<T2>& 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() - SGVec3<T>(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 T1, typename T2>
+inline bool
+intersects(const SGSphere<T1>& sphere, const SGTriangle<T2>& tri)
+{ return intersects(tri, sphere); }
+
+
template<typename T>
inline bool
intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
// See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
SGVec3<T> c = lineSegment.getCenter() - box.getCenter();
- SGVec3<T> w = 0.5*lineSegment.getDirection();
+ SGVec3<T> w = T(0.5)*lineSegment.getDirection();
SGVec3<T> v(fabs(w.x()), fabs(w.y()), fabs(w.z()));
- SGVec3<T> h = 0.5*box.getSize();
+ SGVec3<T> h = T(0.5)*box.getSize();
if (fabs(c[0]) > v[0] + h[0])
return false;
intersects(const SGRay<T>& ray, const SGBox<T>& box)
{ return intersects(box, ray); }
+template<typename T1, typename T2>
+inline bool
+intersects(const SGBox<T1>& box1, const SGBox<T2>& box2)
+{
+ if (box2.getMax()[0] < box1.getMin()[0])
+ return false;
+ if (box1.getMax()[0] < box2.getMin()[0])
+ return false;
+
+ if (box2.getMax()[1] < box1.getMin()[1])
+ return false;
+ if (box1.getMax()[1] < box2.getMin()[1])
+ return false;
+
+ if (box2.getMax()[2] < box1.getMin()[2])
+ return false;
+ if (box1.getMax()[2] < box2.getMin()[2])
+ return false;
+
+ return true;
+}
+
#endif
projects / simgear.git / blobdiff