includedir = @includedir@/math
-check_PROGRAMS = SGMathTest
+check_PROGRAMS = SGMathTest SGGeometryTest
TESTS = $(check_PROGRAMS)
SGMathTest_SOURCES = SGMathTest.cxx
SGMathTest_LDADD = libsgmath.a $(base_LIBS)
+SGGeometryTest_SOURCES = SGGeometryTest.cxx
+SGGeometryTest_LDADD = libsgmath.a $(base_LIBS)
lib_LIBRARIES = libsgmath.a
sg_random.h \
sg_types.hxx \
vector.hxx \
+ SGBox.hxx \
SGCMath.hxx \
SGGeoc.hxx \
SGGeod.hxx \
SGGeodesy.hxx \
+ SGGeometry.hxx \
+ SGGeometryFwd.hxx \
+ SGIntersect.hxx \
SGLimits.hxx \
+ SGLineSegment.hxx \
SGMatrix.hxx \
SGMath.hxx \
SGMathFwd.hxx \
SGMisc.hxx \
+ SGPlane.hxx \
SGQuat.hxx \
- SGVec4.hxx \
+ SGRay.hxx \
+ SGSphere.hxx \
+ SGTriangle.hxx \
+ SGVec2.hxx \
SGVec3.hxx \
- SGVec2.hxx
-
+ SGVec4.hxx
libsgmath_a_SOURCES = \
interpolater.cxx \
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGBox_H
+#define SGBox_H
+
+template<typename T>
+class SGBox {
+public:
+ SGBox() :
+ _min(SGLimits<T>::max(), SGLimits<T>::max(), SGLimits<T>::max()),
+ _max(-SGLimits<T>::max(), -SGLimits<T>::max(), -SGLimits<T>::max())
+ { }
+ void setMin(const SGVec3<T>& min)
+ { _min = min; }
+ const SGVec3<T>& getMin() const
+ { return _min; }
+
+ void setMax(const SGVec3<T>& max)
+ { _max = max; }
+ const SGVec3<T>& getMax() const
+ { return _max; }
+
+ // Only works for floating point types
+ SGVec3<T> getCenter() const
+ { return T(0.5)*(_min + _max); }
+
+ // Only valid for nonempty boxes
+ SGVec3<T> getSize() const
+ { return _max - _min; }
+
+ T getVolume() const
+ {
+ if (empty())
+ return 0;
+ return (_max[0] - _min[0])*(_max[1] - _min[1])*(_max[2] - _min[2]);
+ }
+
+ const bool empty() const
+ { return !valid(); }
+
+ bool valid() const
+ {
+ if (_max[0] < _min[0])
+ return false;
+ if (_max[1] < _min[1])
+ return false;
+ if (_max[2] < _min[2])
+ return false;
+ return true;
+ }
+
+ void clear()
+ {
+ _min[0] = SGLimits<T>::max();
+ _min[1] = SGLimits<T>::max();
+ _min[2] = SGLimits<T>::max();
+ _max[0] = -SGLimits<T>::max();
+ _max[1] = -SGLimits<T>::max();
+ _max[2] = -SGLimits<T>::max();
+ }
+
+ void expandBy(const SGVec3<T>& v)
+ { _min = min(_min, v); _max = max(_max, v); }
+
+ void expandBy(const SGBox<T>& b)
+ { _min = min(_min, b._min); _max = max(_max, b._max); }
+
+ // Note that this only works if the box is nonmepty
+ unsigned getBroadestAxis() const
+ {
+ SGVec3d size = getSize();
+ if (size[1] <= size[0] && size[2] <= size[0])
+ return 0;
+ else if (size[2] <= size[1])
+ return 1;
+ else
+ return 2;
+ }
+
+private:
+ SGVec3<T> _min;
+ SGVec3<T> _max;
+};
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGBox<T>& box)
+{ return s << "min = " << box.getMin() << ", max = " << box.getMax(); }
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGGeometry_HXX
+#define SGGeometry_HXX
+
+// Required ...
+#include "SGMath.hxx"
+
+// Make sure all is defined
+#include "SGGeometryFwd.hxx"
+
+// Geometric primitives we know about
+#include "SGBox.hxx"
+#include "SGSphere.hxx"
+#include "SGRay.hxx"
+#include "SGLineSegment.hxx"
+#include "SGPlane.hxx"
+#include "SGTriangle.hxx"
+
+// Intersection tests
+#include "SGIntersect.hxx"
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGGeometryFwd_HXX
+#define SGGeometryFwd_HXX
+
+template<typename T>
+class SGBox;
+typedef SGBox<float> SGBoxf;
+typedef SGBox<double> SGBoxd;
+
+template<typename T>
+class SGSphere;
+typedef SGSphere<float> SGSpheref;
+typedef SGSphere<double> SGSphered;
+
+template<typename T>
+class SGRay;
+typedef SGRay<float> SGRayf;
+typedef SGRay<double> SGRayd;
+
+template<typename T>
+class SGLineSegment;
+typedef SGLineSegment<float> SGLineSegmentf;
+typedef SGLineSegment<double> SGLineSegmentd;
+
+template<typename T>
+class SGPlane;
+typedef SGPlane<float> SGPlanef;
+typedef SGPlane<double> SGPlaned;
+
+template<typename T>
+class SGTriangle;
+typedef SGTriangle<float> SGTrianglef;
+typedef SGTriangle<double> SGTriangled;
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifdef HAVE_CONFIG_H
+# include <simgear_config.h>
+#endif
+
+#include <cstdlib>
+#include <iostream>
+
+#include "SGGeometry.hxx"
+#include "sg_random.h"
+
+template<typename T>
+SGVec3<T> rndVec3(void)
+{
+ return SGVec3<T>(sg_random(), sg_random(), sg_random());
+}
+
+template<typename T>
+bool
+TriangleLineIntersectionTest(void)
+{
+ unsigned nTests = 100000;
+ unsigned failedCount = 0;
+ for (unsigned i = 0; i < nTests; ++i) {
+ SGVec3<T> v0 = rndVec3<T>();
+ SGVec3<T> v1 = rndVec3<T>();
+ SGVec3<T> v2 = rndVec3<T>();
+
+ SGTriangle<T> tri(v0, v1, v2);
+
+ // generate random coeficients
+ T u = 4*sg_random() - 2;
+ T v = 4*sg_random() - 2;
+ T t = 4*sg_random() - 2;
+
+ SGVec3<T> isectpt = v0 + u*(v1 - v0) + v*(v2 - v0);
+
+ SGLineSegment<T> lineSegment;
+ SGVec3<T> dir = rndVec3<T>();
+
+ SGVec3<T> isectres;
+ lineSegment.set(isectpt - t*dir, isectpt + (1 - t)*dir);
+
+ if (intersects(isectres, tri, lineSegment)) {
+ if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
+ if (!equivalent(isectres, isectpt)) {
+ std::cout << "Failed line segment intersection test #" << i
+ << ": not equivalent!\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ } else {
+ std::cout << "Failed line segment intersection test #" << i
+ << ": false positive!\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t && t <= 1) {
+ std::cout << "Failed line segment intersection test #" << i
+ << ": false negative!\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ }
+
+ SGRay<T> ray;
+ ray.set(isectpt - t*dir, dir);
+ if (intersects(isectres, tri, ray)) {
+ if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
+ if (!equivalent(isectres, isectpt)) {
+ std::cout << "Failed ray intersection test #" << i
+ << ": not equivalent!\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ } else {
+ std::cout << "Failed ray intersection test #" << i
+ << ": false positive!\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (0 <= u && 0 <= v && u+v <= 1 && 0 <= t) {
+ std::cout << "Failed ray intersection test #" << i
+ << ": false negative !\nu = "
+ << u << ", v = " << v << ", t = " << t
+ << "\n" << tri << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ }
+ }
+
+ if (nTests < 100*failedCount) {
+ std::cout << "Failed ray intersection tests: " << failedCount
+ << " tests out of " << nTests
+ << " went wrong. Abort!" << std::endl;
+ return false;
+ }
+
+ /// Some crude handmade test
+ SGVec3<T> v0 = SGVec3<T>(0, 0, 0);
+ SGVec3<T> v1 = SGVec3<T>(1, 0, 0);
+ SGVec3<T> v2 = SGVec3<T>(0, 1, 0);
+
+ SGTriangle<T> tri(v0, v1, v2);
+
+ SGRay<T> ray;
+ ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
+ if (!intersects(tri, ray)) {
+ std::cout << "Failed test #1!" << std::endl;
+ return false;
+ }
+
+ ray.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
+ if (!intersects(tri, ray)) {
+ std::cout << "Failed test #2!" << std::endl;
+ return false;
+ }
+
+ SGLineSegment<T> lineSegment;
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0.1, 0.1, -1));
+ if (!intersects(tri, lineSegment)) {
+ std::cout << "Failed test #3!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 0, -1));
+ if (!intersects(tri, lineSegment)) {
+ std::cout << "Failed test #4!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, 1, -1));
+ if (!intersects(tri, lineSegment)) {
+ std::cout << "Failed test #5!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 0, -1));
+ if (!intersects(tri, lineSegment)) {
+ std::cout << "Failed test #6!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -1));
+ if (!intersects(tri, lineSegment)) {
+ std::cout << "Failed test #7!" << std::endl;
+ return false;
+ }
+
+ // is exactly in the plane
+ // FIXME: cannot detect that yet ??
+// lineSegment.set(SGVec3<T>(0, 0, 0), SGVec3<T>(1, 0, 0));
+// if (!intersects(tri, lineSegment)) {
+// std::cout << "Failed test #8!" << std::endl;
+// return false;
+// }
+
+ // is exactly in the plane
+ // FIXME: cannot detect that yet ??
+// lineSegment.set(SGVec3<T>(-1, 0, 0), SGVec3<T>(1, 0, 0));
+// if (!intersects(tri, lineSegment)) {
+// std::cout << "Failed test #9!" << std::endl;
+// return false;
+// }
+
+ // is exactly paralell to the plane
+ // FIXME: cannot detect that yet ??
+// lineSegment.set(SGVec3<T>(-1, 1, 0), SGVec3<T>(1, 1, 0));
+// if (intersects(tri, lineSegment)) {
+// std::cout << "Failed test #10!" << std::endl;
+// return false;
+// }
+
+ // should fail since the line segment poins slightly beyond the triangle
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(1, 1, -0.9));
+ if (intersects(tri, lineSegment)) {
+ std::cout << "Failed test #11!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(0, -0.1, -1));
+ if (intersects(tri, lineSegment)) {
+ std::cout << "Failed test #12!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, -0.1, -1));
+ if (intersects(tri, lineSegment)) {
+ std::cout << "Failed test #13!" << std::endl;
+ return false;
+ }
+
+ lineSegment.set(SGVec3<T>(0, 0, 1), SGVec3<T>(-0.1, 0, -1));
+ if (intersects(tri, lineSegment)) {
+ std::cout << "Failed test #14!" << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+template<typename T>
+bool
+SphereLineIntersectionTest(void)
+{
+ unsigned nTests = 100000;
+ unsigned failedCount = 0;
+ for (unsigned i = 0; i < nTests; ++i) {
+ SGVec3<T> center = rndVec3<T>();
+ T radius = 2*sg_random();
+ SGSphere<T> sphere(center, radius);
+
+ SGVec3<T> offset = normalize(rndVec3<T>());
+ T t = 4*sg_random();
+
+ // This one is the point we use to judge if the test should fail or not
+ SGVec3<T> base = center + t*offset;
+
+ SGVec3<T> per = perpendicular(offset);
+ SGVec3<T> start = base + 4*sg_random()*per;
+ SGVec3<T> end = base - 4*sg_random()*per;
+
+ SGLineSegment<T> lineSegment;
+ lineSegment.set(start, end);
+ if (intersects(sphere, lineSegment)) {
+ if (radius < t) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false positive!\nt = " << t << "\n"
+ << sphere << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (t <= radius) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false negative!\nt = " << t << "\n"
+ << sphere << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ }
+
+ SGRay<T> ray;
+ ray.set(start, end - start);
+ if (intersects(sphere, ray)) {
+ if (radius < t) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false positive!\nt = " << t << "\n"
+ << sphere << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (t <= radius) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false negative!\nt = " << t << "\n"
+ << sphere << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ }
+ }
+
+ if (nTests < 100*failedCount) {
+ std::cout << "Failed sphere line intersection tests: " << failedCount
+ << " tests out of " << nTests
+ << " went wrong. Abort!" << std::endl;
+ return false;
+ }
+
+ failedCount = 0;
+ for (unsigned i = 0; i < nTests; ++i) {
+ SGVec3<T> center = rndVec3<T>();
+ T radius = 2*sg_random();
+ SGSphere<T> sphere(center, radius);
+
+ SGVec3<T> offset = normalize(rndVec3<T>());
+ T t = 4*sg_random();
+
+ // This one is the point we use to judge if the test should fail or not
+ SGVec3<T> base = center + t*offset;
+
+ SGVec3<T> start = base;
+ SGVec3<T> end = base + 2*sg_random()*offset;
+
+ SGLineSegment<T> lineSegment;
+ lineSegment.set(start, end);
+ if (intersects(sphere, lineSegment)) {
+ if (radius < t) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false positive!\nt = " << t << "\n"
+ << sphere << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (t <= radius) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false negative!\nt = " << t << "\n"
+ << sphere << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ }
+
+ SGRay<T> ray;
+ ray.set(start, end - start);
+ if (intersects(sphere, ray)) {
+ if (radius < t) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false positive!\nt = " << t << "\n"
+ << sphere << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (t <= radius) {
+ std::cout << "Failed sphere line intersection test #" << i
+ << ": false negative!\nt = " << t << "\n"
+ << sphere << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ }
+ }
+
+ if (nTests < 100*failedCount) {
+ std::cout << "Failed sphere line intersection tests: " << failedCount
+ << " tests out of " << nTests
+ << " went wrong. Abort!" << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+template<typename T>
+bool
+BoxLineIntersectionTest(void)
+{
+ // ok, bad test case coverage, but better than nothing ...
+
+ unsigned nTests = 100000;
+ unsigned failedCount = 0;
+ for (unsigned i = 0; i < nTests; ++i) {
+ SGBox<T> box;
+ box.expandBy(rndVec3<T>());
+ box.expandBy(rndVec3<T>());
+
+ SGVec3<T> center = box.getCenter();
+
+ // This one is the point we use to judge if the test should fail or not
+ SGVec3<T> base = rndVec3<T>();
+ SGVec3<T> dir = base - center;
+
+ SGLineSegment<T> lineSegment;
+ lineSegment.set(base, base + dir);
+ if (intersects(box, lineSegment)) {
+ if (!intersects(box, base)) {
+ std::cout << "Failed box line intersection test #" << i
+ << ": false positive!\n"
+ << box << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (intersects(box, base)) {
+ std::cout << "Failed box line intersection test #" << i
+ << ": false negative!\n"
+ << box << "\n" << lineSegment << std::endl;
+ ++failedCount;
+ }
+ }
+
+ SGRay<T> ray;
+ ray.set(base, dir);
+ if (intersects(box, ray)) {
+ if (!intersects(box, base)) {
+ std::cout << "Failed box line intersection test #" << i
+ << ": false positive!\n"
+ << box << "\n" << ray << std::endl;
+ ++failedCount;
+ }
+ } else {
+ if (intersects(box, base)) {
+ std::cout << "Failed box line intersection test #" << i
+ << ": false negative!\n"
+ << box << "\n" << ray << 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;
+}
+
+int
+main(void)
+{
+ std::cout << "Testing Geometry intersection routines.\n"
+ << "Some of these tests can fail due to roundoff problems...\n"
+ << "Dont worry if only a few of them fail..." << std::endl;
+
+ if (!TriangleLineIntersectionTest<float>())
+ return EXIT_FAILURE;
+ if (!TriangleLineIntersectionTest<double>())
+ return EXIT_FAILURE;
+
+ if (!SphereLineIntersectionTest<float>())
+ return EXIT_FAILURE;
+ if (!SphereLineIntersectionTest<double>())
+ return EXIT_FAILURE;
+
+ if (!BoxLineIntersectionTest<float>())
+ return EXIT_FAILURE;
+ if (!BoxLineIntersectionTest<double>())
+ return EXIT_FAILURE;
+
+ std::cout << "Successfully passed all tests!" << std::endl;
+ return EXIT_SUCCESS;
+}
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGIntersect_HXX
+#define SGIntersect_HXX
+
+template<typename T>
+inline bool
+intersects(const SGBox<T>& box, const SGSphere<T>& sphere)
+{
+ if (sphere.empty())
+ return false;
+ // Is more or less trivially included in the next tests
+ // if (box.empty())
+ // return false;
+
+ if (sphere.getCenter().x() < box.getMin().x() - sphere.getRadius())
+ return false;
+ if (sphere.getCenter().y() < box.getMin().y() - sphere.getRadius())
+ return false;
+ if (sphere.getCenter().z() < box.getMin().z() - sphere.getRadius())
+ return false;
+
+ if (box.getMax().x() + sphere.getRadius() < sphere.getCenter().x())
+ return false;
+ if (box.getMax().y() + sphere.getRadius() < sphere.getCenter().y())
+ return false;
+ if (box.getMax().z() + sphere.getRadius() < sphere.getCenter().z())
+ return false;
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGSphere<T>& sphere, const SGBox<T>& box)
+{ return intersects(box, sphere); }
+
+
+template<typename T>
+inline bool
+intersects(const SGVec3<T>& v, const SGBox<T>& box)
+{
+ if (v[0] < box.getMin()[0])
+ return false;
+ if (box.getMax()[0] < v[0])
+ return false;
+ if (v[1] < box.getMin()[1])
+ return false;
+ if (box.getMax()[1] < v[1])
+ return false;
+ if (v[2] < box.getMin()[2])
+ return false;
+ if (box.getMax()[2] < v[2])
+ return false;
+ return true;
+}
+template<typename T>
+inline bool
+intersects(const SGBox<T>& box, const SGVec3<T>& v)
+{ return intersects(v, box); }
+
+
+template<typename T>
+inline bool
+intersects(const SGRay<T>& ray, const SGPlane<T>& plane)
+{
+ // We compute the intersection point
+ // x = origin + \alpha*direction
+ // from the ray origin and non nomalized direction.
+ // For 0 <= \alpha the ray intersects the infinite plane.
+ // The intersection point x can also be written
+ // x = n*dist + y
+ // where n is the planes normal, dist is the distance of the plane from
+ // the origin in normal direction and y is ana aproriate vector
+ // perpendicular to n.
+ // Equate the x values and take the scalar product with the plane normal n.
+ // dot(n, origin) + \alpha*dot(n, direction) = dist
+ // We can now compute alpha from the above equation.
+ // \alpha = (dist - dot(n, origin))/dot(n, direction)
+
+ // The negative numerator for the \alpha expression
+ T num = plane.getPositiveDist();
+ num -= dot(plane.getNormal(), ray.getOrigin());
+
+ // If the numerator is zero, we have the rays origin included in the plane
+ if (fabs(num) <= SGLimits<T>::min())
+ return true;
+
+ // The denominator for the \alpha expression
+ T den = dot(plane.getNormal(), ray.getDirection());
+
+ // If we get here, we already know that the rays origin is not included
+ // in the plane. Thus if we have a zero denominator we have
+ // a ray paralell to the plane. That is no intersection.
+ if (fabs(den) <= SGLimits<T>::min())
+ return false;
+
+ // We would now compute \alpha = num/den and compare with 0 and 1.
+ // But to avoid that expensive division, check equation multiplied by
+ // the denominator.
+ T alphaDen = copysign(1, den)*num;
+ if (alphaDen < 0)
+ return false;
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGPlane<T>& plane, const SGRay<T>& ray)
+{ return intersects(ray, plane); }
+
+template<typename T>
+inline bool
+intersects(SGVec3<T>& dst, const SGRay<T>& ray, const SGPlane<T>& plane)
+{
+ // We compute the intersection point
+ // x = origin + \alpha*direction
+ // from the ray origin and non nomalized direction.
+ // For 0 <= \alpha the ray intersects the infinite plane.
+ // The intersection point x can also be written
+ // x = n*dist + y
+ // where n is the planes normal, dist is the distance of the plane from
+ // the origin in normal direction and y is ana aproriate vector
+ // perpendicular to n.
+ // Equate the x values and take the scalar product with the plane normal n.
+ // dot(n, origin) + \alpha*dot(n, direction) = dist
+ // We can now compute alpha from the above equation.
+ // \alpha = (dist - dot(n, origin))/dot(n, direction)
+
+ // The negative numerator for the \alpha expression
+ T num = plane.getPositiveDist();
+ num -= dot(plane.getNormal(), ray.getOrigin());
+
+ // If the numerator is zero, we have the rays origin included in the plane
+ if (fabs(num) <= SGLimits<T>::min()) {
+ dst = ray.getOrigin();
+ return true;
+ }
+
+ // The denominator for the \alpha expression
+ T den = dot(plane.getNormal(), ray.getDirection());
+
+ // If we get here, we already know that the rays origin is not included
+ // in the plane. Thus if we have a zero denominator we have
+ // a ray paralell to the plane. That is no intersection.
+ if (fabs(den) <= SGLimits<T>::min())
+ return false;
+
+ // We would now compute \alpha = num/den and compare with 0 and 1.
+ // But to avoid that expensive division, check equation multiplied by
+ // the denominator.
+ T alpha = num/den;
+ if (alpha < 0)
+ return false;
+
+ dst = ray.getOrigin() + alpha*ray.getDirection();
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(SGVec3<T>& dst, const SGPlane<T>& plane, const SGRay<T>& ray)
+{ return intersects(dst, ray, plane); }
+
+template<typename T>
+inline bool
+intersects(const SGLineSegment<T>& lineSegment, const SGPlane<T>& plane)
+{
+ // We compute the intersection point
+ // x = origin + \alpha*direction
+ // from the line segments origin and non nomalized direction.
+ // For 0 <= \alpha <= 1 the line segment intersects the infinite plane.
+ // The intersection point x can also be written
+ // x = n*dist + y
+ // where n is the planes normal, dist is the distance of the plane from
+ // the origin in normal direction and y is ana aproriate vector
+ // perpendicular to n.
+ // Equate the x values and take the scalar product with the plane normal n.
+ // dot(n, origin) + \alpha*dot(n, direction) = dist
+ // We can now compute alpha from the above equation.
+ // \alpha = (dist - dot(n, origin))/dot(n, direction)
+
+ // The negative numerator for the \alpha expression
+ T num = plane.getPositiveDist();
+ num -= dot(plane.getNormal(), lineSegment.getOrigin());
+
+ // If the numerator is zero, we have the lines origin included in the plane
+ if (fabs(num) <= SGLimits<T>::min())
+ return true;
+
+ // The denominator for the \alpha expression
+ T den = dot(plane.getNormal(), lineSegment.getDirection());
+
+ // If we get here, we already know that the lines origin is not included
+ // in the plane. Thus if we have a zero denominator we have
+ // a line paralell to the plane. That is no intersection.
+ if (fabs(den) <= SGLimits<T>::min())
+ return false;
+
+ // We would now compute \alpha = num/den and compare with 0 and 1.
+ // But to avoid that expensive division, compare equations
+ // multiplied by |den|. Note that copysign is usually a compiler intrinsic
+ // that expands in assembler code that not even stalls the cpus pipes.
+ T alphaDen = copysign(1, den)*num;
+ if (alphaDen < 0)
+ return false;
+ if (den < alphaDen)
+ return false;
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGPlane<T>& plane, const SGLineSegment<T>& lineSegment)
+{ return intersects(lineSegment, plane); }
+
+template<typename T>
+inline bool
+intersects(SGVec3<T>& dst, const SGLineSegment<T>& lineSegment, const SGPlane<T>& plane)
+{
+ // We compute the intersection point
+ // x = origin + \alpha*direction
+ // from the line segments origin and non nomalized direction.
+ // For 0 <= \alpha <= 1 the line segment intersects the infinite plane.
+ // The intersection point x can also be written
+ // x = n*dist + y
+ // where n is the planes normal, dist is the distance of the plane from
+ // the origin in normal direction and y is an aproriate vector
+ // perpendicular to n.
+ // Equate the x values and take the scalar product with the plane normal n.
+ // dot(n, origin) + \alpha*dot(n, direction) = dist
+ // We can now compute alpha from the above equation.
+ // \alpha = (dist - dot(n, origin))/dot(n, direction)
+
+ // The negative numerator for the \alpha expression
+ T num = plane.getPositiveDist();
+ num -= dot(plane.getNormal(), lineSegment.getOrigin());
+
+ // If the numerator is zero, we have the lines origin included in the plane
+ if (fabs(num) <= SGLimits<T>::min()) {
+ dst = lineSegment.getOrigin();
+ return true;
+ }
+
+ // The denominator for the \alpha expression
+ T den = dot(plane.getNormal(), lineSegment.getDirection());
+
+ // If we get here, we already know that the lines origin is not included
+ // in the plane. Thus if we have a zero denominator we have
+ // a line paralell to the plane. That is: no intersection.
+ if (fabs(den) <= SGLimits<T>::min())
+ return false;
+
+ // We would now compute \alpha = num/den and compare with 0 and 1.
+ // But to avoid that expensive division, check equation multiplied by
+ // the denominator. FIXME: shall we do so? or compute like that?
+ T alpha = num/den;
+ if (alpha < 0)
+ return false;
+ if (1 < alpha)
+ return false;
+
+ dst = lineSegment.getOrigin() + alpha*lineSegment.getDirection();
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(SGVec3<T>& dst, const SGPlane<T>& plane, const SGLineSegment<T>& lineSegment)
+{ return intersects(dst, lineSegment, plane); }
+
+
+template<typename T>
+inline bool
+intersects(const SGRay<T>& ray, const SGSphere<T>& sphere)
+{
+ // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering,
+ // second edition, page 571
+ SGVec3<T> l = sphere.getCenter() - ray.getOrigin();
+ T s = dot(l, ray.getDirection());
+ T l2 = dot(l, l);
+
+ T r2 = sphere.getRadius2();
+ if (s < 0 && l2 > r2)
+ return false;
+
+ T d2 = dot(ray.getDirection(), ray.getDirection());
+ // The original test would read
+ // T m2 = l2 - s*s/d2;
+ // if (m2 > r2)
+ // return false;
+ // but to avoid the expensive division, we multiply by d2
+ T m2 = d2*l2 - s*s;
+ if (m2 > d2*r2)
+ return false;
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGSphere<T>& sphere, const SGRay<T>& ray)
+{ return intersects(ray, sphere); }
+
+template<typename T>
+inline bool
+intersects(const SGLineSegment<T>& lineSegment, const SGSphere<T>& sphere)
+{
+ // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering,
+ // second edition, page 571
+ SGVec3<T> l = sphere.getCenter() - lineSegment.getStart();
+ T ld = length(lineSegment.getDirection());
+ T s = dot(l, lineSegment.getDirection())/ld;
+ T l2 = dot(l, l);
+
+ T r2 = sphere.getRadius2();
+ if (s < 0 && l2 > r2)
+ return false;
+
+ T m2 = l2 - s*s;
+ if (m2 > r2)
+ return false;
+
+ T q = sqrt(r2 - m2);
+ T t = s - q;
+ if (ld < t)
+ return false;
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGSphere<T>& sphere, const SGLineSegment<T>& lineSegment)
+{ return intersects(lineSegment, sphere); }
+
+
+template<typename T>
+inline bool
+// FIXME do not use that default argument later. Just for development now
+intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
+{
+ // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
+
+ // Method based on the observation that we are looking for a
+ // point x that can be expressed in terms of the triangle points
+ // x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
+ // with 0 <= u, v and u + v <= 1.
+ // OTOH it could be expressed in terms of the ray
+ // x = o + t*d
+ // Now we can compute u, v and t.
+ SGVec3<T> p = cross(ray.getDirection(), tri.getEdge(1));
+
+ T denom = dot(p, tri.getEdge(0));
+ T signDenom = copysign(1, denom);
+
+ SGVec3<T> s = ray.getOrigin() - tri.getBaseVertex();
+ SGVec3<T> q = cross(s, tri.getEdge(0));
+ // Now t would read
+ // t = 1/denom*dot(q, tri.getEdge(1));
+ // To avoid an expensive division we multiply by |denom|
+ T tDenom = signDenom*dot(q, tri.getEdge(1));
+ if (tDenom < 0)
+ return false;
+ // For line segment we would test against
+ // if (1 < t)
+ // return false;
+ // with the original t. The multiplied test would read
+ // if (absDenom < tDenom)
+ // return false;
+
+ T absDenom = fabs(denom);
+ T absDenomEps = absDenom*eps;
+
+ // T u = 1/denom*dot(p, s);
+ T u = signDenom*dot(p, s);
+ if (u < -absDenomEps)
+ return false;
+ // T v = 1/denom*dot(q, d);
+ // if (v < -eps)
+ // return false;
+ T v = signDenom*dot(q, ray.getDirection());
+ if (v < -absDenomEps)
+ return false;
+
+ if (u + v > absDenom + absDenomEps)
+ return false;
+
+ // return if paralell ??? FIXME what if paralell and in plane?
+ // may be we are ok below than anyway??
+ if (absDenom <= SGLimits<T>::min())
+ return false;
+
+ x = ray.getOrigin();
+ // if we have survived here it could only happen with denom == 0
+ // that the point is already in plane. Then return the origin ...
+ if (SGLimitsd::min() < absDenom)
+ x += (tDenom/absDenom)*ray.getDirection();
+
+ return true;
+}
+
+template<typename T>
+inline bool
+intersects(const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
+{
+ // 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, ray, eps);
+}
+
+template<typename T>
+inline bool
+// FIXME do not use that default argument later. Just for development now
+intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
+{
+ // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
+
+ // Method based on the observation that we are looking for a
+ // point x that can be expressed in terms of the triangle points
+ // x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
+ // with 0 <= u, v and u + v <= 1.
+ // OTOH it could be expressed in terms of the lineSegment
+ // x = o + t*d
+ // Now we can compute u, v and t.
+ SGVec3<T> p = cross(lineSegment.getDirection(), tri.getEdge(1));
+
+ T denom = dot(p, tri.getEdge(0));
+ T signDenom = copysign(1, denom);
+
+ SGVec3<T> s = lineSegment.getStart() - tri.getBaseVertex();
+ SGVec3<T> q = cross(s, tri.getEdge(0));
+ // Now t would read
+ // t = 1/denom*dot(q, tri.getEdge(1));
+ // To avoid an expensive division we multiply by |denom|
+ T tDenom = signDenom*dot(q, tri.getEdge(1));
+ if (tDenom < 0)
+ return false;
+ // For line segment we would test against
+ // if (1 < t)
+ // return false;
+ // with the original t. The multiplied test reads
+ T absDenom = fabs(denom);
+ if (absDenom < tDenom)
+ return false;
+
+ // take the CPU accuracy in account
+ T absDenomEps = absDenom*eps;
+
+ // T u = 1/denom*dot(p, s);
+ T u = signDenom*dot(p, s);
+ if (u < -absDenomEps)
+ return false;
+ // T v = 1/denom*dot(q, d);
+ // if (v < -eps)
+ // return false;
+ T v = signDenom*dot(q, lineSegment.getDirection());
+ if (v < -absDenomEps)
+ return false;
+
+ if (u + v > absDenom + absDenomEps)
+ return false;
+
+ // return if paralell ??? FIXME what if paralell and in plane?
+ // may be we are ok below than anyway??
+ if (absDenom <= SGLimits<T>::min())
+ return false;
+
+ x = lineSegment.getStart();
+ // if we have survived here it could only happen with denom == 0
+ // that the point is already in plane. Then return the origin ...
+ if (SGLimitsd::min() < absDenom)
+ x += (tDenom/absDenom)*lineSegment.getDirection();
+
+ return true;
+}
+
+template<typename T>
+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
+ // well optimized, implement that special case
+ SGVec3<T> dummy;
+ return intersects(dummy, tri, lineSegment, eps);
+}
+
+
+template<typename T>
+inline bool
+intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
+{
+ if (sphere.empty())
+ return false;
+ return distSqr(v, sphere.getCenter()) <= sphere.getRadius2();
+}
+template<typename T>
+inline bool
+intersects(const SGSphere<T>& sphere, const SGVec3<T>& v)
+{ return intersects(v, sphere); }
+
+
+template<typename T>
+inline bool
+intersects(const SGBox<T>& box, const SGLineSegment<T>& lineSegment)
+{
+ // 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> v(fabs(w.x()), fabs(w.y()), fabs(w.z()));
+ SGVec3<T> h = 0.5*box.getSize();
+
+ if (fabs(c[0]) > v[0] + h[0])
+ return false;
+ if (fabs(c[1]) > v[1] + h[1])
+ return false;
+ if (fabs(c[2]) > v[2] + h[2])
+ return false;
+
+ if (fabs(c[1]*w[2] - c[2]*w[1]) > h[1]*v[2] + h[2]*v[1])
+ return false;
+ if (fabs(c[0]*w[2] - c[2]*w[0]) > h[0]*v[2] + h[2]*v[0])
+ return false;
+ if (fabs(c[0]*w[1] - c[1]*w[0]) > h[0]*v[1] + h[1]*v[0])
+ return false;
+
+ return true;
+}
+template<typename T>
+inline bool
+intersects(const SGLineSegment<T>& lineSegment, const SGBox<T>& box)
+{ return intersects(box, lineSegment); }
+
+template<typename T>
+inline bool
+intersects(const SGBox<T>& box, const SGRay<T>& ray)
+{
+ // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
+
+ for (unsigned i = 0; i < 3; ++i) {
+ T cMin = box.getMin()[i];
+ T cMax = box.getMax()[i];
+
+ T cOrigin = ray.getOrigin()[i];
+
+ T cDir = ray.getDirection()[i];
+ if (fabs(cDir) <= SGLimits<T>::min()) {
+ if (cOrigin < cMin)
+ return false;
+ if (cMax < cOrigin)
+ return false;
+ }
+
+ T near = - SGLimits<T>::max();
+ T far = SGLimits<T>::max();
+
+ T T1 = (cMin - cOrigin) / cDir;
+ T T2 = (cMax - cOrigin) / cDir;
+ if (T1 > T2) std::swap (T1, T2);/* since T1 intersection with near plane */
+ if (T1 > near) near = T1; /* want largest Tnear */
+ if (T2 < far) far = T2; /* want smallest Tfar */
+ if (near > far) // far box is missed
+ return false;
+ if (far < 0) // box is behind ray
+ return false;
+ }
+
+ return true;
+}
+// make it symmetric
+template<typename T>
+inline bool
+intersects(const SGRay<T>& ray, const SGBox<T>& box)
+{ return intersects(box, ray); }
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGLineSegment_H
+#define SGLineSegment_H
+
+template<typename T>
+class SGLineSegment {
+public:
+ SGLineSegment()
+ { }
+ SGLineSegment(const SGVec3<T>& start, const SGVec3<T>& end) :
+ _start(start),
+ _direction(end - start)
+ { }
+
+ void set(const SGVec3<T>& start, const SGVec3<T>& end)
+ { _start = start; _direction = end - start; }
+
+ const SGVec3<T>& getStart() const
+ { return _start; }
+ SGVec3<T> getEnd() const
+ { return _start + _direction; }
+ const SGVec3<T>& getDirection() const
+ { return _direction; }
+ SGVec3<T> getNormalizedDirection() const
+ { return normalize(getDirection()); }
+
+ SGVec3<T> getCenter() const
+ { return _start + T(0.5)*_direction; }
+
+private:
+ SGVec3<T> _start;
+ SGVec3<T> _direction;
+};
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s,
+ const SGLineSegment<T>& lineSegment)
+{
+ return s << "line segment: start = " << lineSegment.getStart()
+ << ", end = " << lineSegment.getEnd();
+}
+
+#endif
// Create some test matrix
SGVec3<T> v0(2, 7, 17);
SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
- SGMatrix<T> m0(q0, v0);
+ SGMatrix<T> m0;
+ m0.postMultTranslate(v0);
+ m0.postMultRotate(q0);
// Check the tqo forms of the inverse for that kind of special matrix
SGMatrix<T> m1, m2;
SGVec3f v3f = SGVec3f::e2();
SGVec4f v4f = SGVec4f::e2();
SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
- SGMatrixf mf(qf, v3f);
+ SGMatrixf mf;
+ mf.postMultTranslate(v3f);
+ mf.postMultRotate(qf);
// Copy to and from plibs types check if result is equal,
// test for exact equality
SGVec3d v3d = SGVec3d::e2();
SGVec4d v4d = SGVec4d::e2();
SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
- SGMatrixd md(qd, v3d);
+ SGMatrixd md;
+ md.postMultTranslate(v3d);
+ md.postMultRotate(qd);
// Copy to and from plibs types check if result is equal,
// test for exact equality
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGPlane_H
+#define SGPlane_H
+
+template<typename T>
+class SGPlane {
+public:
+ SGPlane()
+ { }
+ SGPlane(const SGVec3<T>& normal, T dist) :
+ _normal(normal), _dist(dist)
+ { }
+ SGPlane(const SGVec3<T> vertices[3]) :
+ _normal(normalize(cross(vertices[1] - vertices[0],
+ vertices[2] - vertices[0]))),
+ _dist(-dot(_normal, vertices[0]))
+ { }
+
+ void setNormal(const SGVec3<T>& normal)
+ { _normal = normal; }
+ const SGVec3<T>& getNormal() const
+ { return _normal; }
+
+ void setDist(const T& dist)
+ { _dist = dist; }
+ const T& getDist() const
+ { return _dist; }
+
+ /// That is the distance where we measure positive in direction of the normal
+ T getPositiveDist() const
+ { return -_dist; }
+ /// That is the distance where we measure positive in the oposite direction
+ /// of the normal.
+ const T& getNegativeDist() const
+ { return _dist; }
+
+private:
+ // That ordering is important because of one constructor
+ SGVec3<T> _normal;
+ T _dist;
+};
+
+#endif
static SGQuat fromHeadAttBankDeg(T h, T a, T b)
{ return fromEulerDeg(h, a, b); }
- /// Return a quaternion rotation the the horizontal local frame from given
- /// longitude and latitude
+ /// Return a quaternion rotation from the earth centered to the
+ /// simulation usual horizontal local frame from given
+ /// longitude and latitude.
+ /// The horizontal local frame used in simulations is the frame with x-axis
+ /// pointing north, the y-axis pointing eastwards and the z axis
+ /// pointing downwards.
static SGQuat fromLonLatRad(T lon, T lat)
{
SGQuat q;
T zd2 = T(0.5)*lon;
- T yd2 = T(-0.25)*SGMisc<value_type>::pi() - T(0.5)*lat;
+ T yd2 = T(-0.25)*SGMisc<T>::pi() - T(0.5)*lat;
T Szd2 = sin(zd2);
T Syd2 = sin(yd2);
T Czd2 = cos(zd2);
q.z() = Szd2*Cyd2;
return q;
}
-
- /// Return a quaternion rotation the the horizontal local frame from given
- /// longitude and latitude
+ /// Like the above provided for convenience
static SGQuat fromLonLatDeg(T lon, T lat)
{ return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
-
- /// Return a quaternion rotation the the horizontal local frame from given
- /// longitude and latitude
+ /// Like the above provided for convenience
static SGQuat fromLonLat(const SGGeod& geod)
{ return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
+ /// Return a quaternion rotation from the earth centered to the
+ /// OpenGL/viewer horizontal local frame from given longitude and latitude.
+ /// This frame matches the usual OpenGL axis directions. That is the target
+ /// frame has an x-axis pointing eastwards, y-axis pointing up and y z-axis
+ /// pointing south.
+ static SGQuat viewHLRad(T lon, T lat)
+ {
+ // That bails down to a 3-2-1 euler sequence lon+pi/2, 0, -lat-pi
+ // what is here is again the hand optimized version ...
+ SGQuat q;
+ T xd2 = -T(0.5)*lat - T(0.5)*SGMisc<T>::pi();
+ T zd2 = T(0.5)*lon + T(0.25)*SGMisc<T>::pi();
+ T Szd2 = sin(zd2);
+ T Sxd2 = sin(xd2);
+ T Czd2 = cos(zd2);
+ T Cxd2 = cos(xd2);
+ q.w() = Cxd2*Czd2;
+ q.x() = Sxd2*Czd2;
+ q.y() = Sxd2*Szd2;
+ q.z() = Cxd2*Szd2;
+ return q;
+ }
+ /// Like the above provided for convenience
+ static SGQuat viewHLDeg(T lon, T lat)
+ { return viewHLRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
+ /// Like the above provided for convenience
+ static SGQuat viewHL(const SGGeod& geod)
+ { return viewHLRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
+
+ /// Convert a quaternion rotation from the simulation frame
+ /// to the view (OpenGL) frame. That is it just swaps the axis part of
+ /// this current quaternion.
+ /// That proves useful when you want to use the euler 3-2-1 sequence
+ /// for the usual heading/pitch/roll sequence within the context of
+ /// OpenGL/viewer frames.
+ static SGQuat simToView(const SGQuat& q)
+ { return SGQuat(q.y(), -q.z(), -q.x(), q.w()); }
+
/// Create a quaternion from the angle axis representation
static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
{
static SGQuat fromChangeSign(const SGVec3<T>& v)
{
// The vector from points to the oposite direction than to.
- // Find a vector perpandicular to the vector to.
+ // Find a vector perpendicular to the vector to.
T absv1 = fabs(v(0));
T absv2 = fabs(v(1));
T absv3 = fabs(v(2));
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGRay_H
+#define SGRay_H
+
+template<typename T>
+class SGRay {
+public:
+ SGRay()
+ { }
+ SGRay(const SGVec3<T>& origin, const SGVec3<T>& dir) :
+ _origin(origin), _direction(dir)
+ { }
+
+ void set(const SGVec3<T>& origin, const SGVec3<T>& dir)
+ { _origin = origin; _direction = dir; }
+
+ void setOrigin(const SGVec3<T>& origin)
+ { _origin = origin; }
+ const SGVec3<T>& getOrigin() const
+ { return _origin; }
+
+ void setDirection(const SGVec3<T>& direction)
+ { _direction = direction; }
+ const SGVec3<T>& getDirection() const
+ { return _direction; }
+
+ SGVec3<T> getNormalizedDirection() const
+ { return normalize(getDirection()); }
+
+private:
+ SGVec3<T> _origin;
+ SGVec3<T> _direction;
+};
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s,
+ const SGRay<T>& ray)
+{
+ return s << "ray: origin = " << ray.getOrigin()
+ << ", direction = " << ray.getDirection();
+}
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGSphere_H
+#define SGSphere_H
+
+template<typename T>
+class SGSphere {
+public:
+ SGSphere() :
+ _radius(-1)
+ { }
+ SGSphere(const SGVec3<T>& center, const T& radius) :
+ _center(center),
+ _radius(radius)
+ { }
+
+ const SGVec3<T>& getCenter() const
+ { return _center; }
+ void setCenter(const SGVec3<T>& center)
+ { _center = center; }
+
+ const T& getRadius() const
+ { return _radius; }
+ void setRadius(const T& radius)
+ { _radius = radius; }
+ T getRadius2() const
+ { return _radius*_radius; }
+
+ const bool empty() const
+ { return !valid(); }
+
+ bool valid() const
+ { return 0 <= _radius; }
+
+ void clear()
+ { _radius = -1; }
+
+ void expandBy(const SGVec3<T>& v)
+ {
+ if (empty()) {
+ _center = v;
+ _radius = 0;
+ return;
+ }
+
+ T dist2 = distSqr(_center, v);
+ if (dist2 <= getRadius2())
+ return;
+
+ T dist = sqrt(dist2);
+ T newRadius = T(0.5)*(_radius + dist);
+ _center += ((newRadius - _radius)/dist)*(v - _center);
+ _radius = newRadius;
+ }
+
+private:
+ SGVec3<T> _center;
+ T _radius;
+};
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s,
+ const SGSphere<T>& sphere)
+{
+ return s << "center = " << sphere.getCenter()
+ << ", radius = " << sphere.getRadius();
+}
+
+#endif
--- /dev/null
+// Copyright (C) 2006 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
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef SGTriangle_H
+#define SGTrianlge_H
+
+template<typename T>
+class SGTriangle {
+public:
+ SGTriangle()
+ { }
+ SGTriangle(const SGVec3<T>& v0, const SGVec3<T>& v1, const SGVec3<T>& v2)
+ { set(v0, v1, v2); }
+ SGTriangle(const SGVec3<T> v[3])
+ { set(v); }
+
+ void set(const SGVec3<T>& v0, const SGVec3<T>& v1, const SGVec3<T>& v2)
+ {
+ _v0 = v0;
+ _d[0] = v1 - v0;
+ _d[1] = v2 - v0;
+ }
+ void set(const SGVec3<T> v[3])
+ {
+ _v0 = v[0];
+ _d[0] = v[1] - v[0];
+ _d[1] = v[2] - v[0];
+ }
+
+ SGVec3d getCenter() const
+ {
+ SGBoxd box;
+ box.expandBy(_v0);
+ box.expandBy(_v0 + _d[0]);
+ box.expandBy(_v0 + _d[1]);
+ return box.getCenter();
+ }
+
+ // note that the index is unchecked
+ SGVec3<T> getVertex(unsigned i) const
+ {
+ if (0 < i)
+ return _v0 + _d[i-1];
+ return _v0;
+ }
+ /// return the normalized surface normal
+ SGVec3<T> getNormal() const
+ { return normalize(cross(_d[0], _d[1])); }
+
+ const SGVec3<T>& getBaseVertex() const
+ { return _v0; }
+ void setBaseVertex(const SGVec3<T>& v)
+ { _v0 = v; }
+ const SGVec3<T>& getEdge(unsigned i) const
+ { return _d[i]; }
+ void setEdge(unsigned i, const SGVec3<T>& d)
+ { _d[i] = d; }
+
+ // flip the positive side
+ void flip()
+ {
+ SGVec3<T> tmp = _d[0];
+ _d[0] = _d[1];
+ _d[1] = tmp;
+ }
+private:
+ /// Store one vertex directly, _d is the offset of the other two
+ /// vertices wrt the base vertex
+ /// For fast intersection tests this format prooves usefull. For that same
+ /// purpose also cache the cross product of the _d[i].
+ SGVec3<T> _v0;
+ SGVec3<T> _d[2];
+};
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s,
+ const SGTriangle<T>& triangle)
+{
+ return s << "triangle: v0 = " << triangle.getVertex(0)
+ << ", v1 = " << triangle.getVertex(1)
+ << ", v2 = " << triangle.getVertex(2);
+}
+
+#endif
v1(0)*v2(1) - v1(1)*v2(0));
}
-/// return any vector perpendicular to v
+/// return any normalized vector perpendicular to v
template<typename T>
inline
SGVec3<T>
perpendicular(const SGVec3<T>& v)
{
- if (fabs(v.x()) < fabs(v.y()) && fabs(v.x()) < fabs(v.z()))
- return cross(SGVec3f(1, 0, 0), v);
- else if (fabs(v.y()) < fabs(v.x()) && fabs(v.y()) < fabs(v.z()))
- return cross(SGVec3f(0, 1, 0), v);
- else
- return cross(SGVec3f(0, 0, 1), v);
+ T absv1 = fabs(v(0));
+ T absv2 = fabs(v(1));
+ T absv3 = fabs(v(2));
+
+ if (absv2 < absv1 && absv3 < absv1) {
+ T quot = v(1)/v(0);
+ return (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
+ } else if (absv1 < absv2 && absv3 < absv2) {
+ T quot = v(2)/v(1);
+ return (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
+ } else if (absv1 < absv3 && absv2 < absv3) {
+ T quot = v(0)/v(2);
+ return (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
+ } else {
+ // the all zero case ...
+ return SGVec3<T>(0, 0, 0);
+ }
}
/// The euclidean norm of the vector, that is what most people call length
projects / simgear.git / commitdiff