From de020ee69524393daf11200aa0a46bfd5aa2409a Mon Sep 17 00:00:00 2001 From: frohlich Date: Wed, 27 Dec 2006 09:23:39 +0000 Subject: [PATCH] Modified Files: Makefile.am SGMathTest.cxx SGQuat.hxx SGVec3.hxx Added Files: SGBox.hxx SGGeometry.hxx SGGeometryFwd.hxx SGGeometryTest.cxx SGIntersect.hxx SGLineSegment.hxx SGPlane.hxx SGRay.hxx SGSphere.hxx SGTriangle.hxx: Small updates to the vector code, new geometry and collision classes for use with a bv tree to speed up collission tests. Also included is a rought unit test for the collissions. --- simgear/math/Makefile.am | 18 +- simgear/math/SGBox.hxx | 107 ++++++ simgear/math/SGGeometry.hxx | 38 ++ simgear/math/SGGeometryFwd.hxx | 51 +++ simgear/math/SGGeometryTest.cxx | 442 ++++++++++++++++++++++++ simgear/math/SGIntersect.hxx | 595 ++++++++++++++++++++++++++++++++ simgear/math/SGLineSegment.hxx | 62 ++++ simgear/math/SGMathTest.cxx | 12 +- simgear/math/SGPlane.hxx | 59 ++++ simgear/math/SGQuat.hxx | 58 +++- simgear/math/SGRay.hxx | 62 ++++ simgear/math/SGSphere.hxx | 87 +++++ simgear/math/SGTriangle.hxx | 101 ++++++ simgear/math/SGVec3.hxx | 25 +- 14 files changed, 1693 insertions(+), 24 deletions(-) create mode 100644 simgear/math/SGBox.hxx create mode 100644 simgear/math/SGGeometry.hxx create mode 100644 simgear/math/SGGeometryFwd.hxx create mode 100644 simgear/math/SGGeometryTest.cxx create mode 100644 simgear/math/SGIntersect.hxx create mode 100644 simgear/math/SGLineSegment.hxx create mode 100644 simgear/math/SGPlane.hxx create mode 100644 simgear/math/SGRay.hxx create mode 100644 simgear/math/SGSphere.hxx create mode 100644 simgear/math/SGTriangle.hxx diff --git a/simgear/math/Makefile.am b/simgear/math/Makefile.am index 016620ee..9aba3599 100644 --- a/simgear/math/Makefile.am +++ b/simgear/math/Makefile.am @@ -1,11 +1,13 @@ 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 @@ -19,20 +21,28 @@ include_HEADERS = \ 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 \ diff --git a/simgear/math/SGBox.hxx b/simgear/math/SGBox.hxx new file mode 100644 index 00000000..272c7cab --- /dev/null +++ b/simgear/math/SGBox.hxx @@ -0,0 +1,107 @@ +// 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 +class SGBox { +public: + SGBox() : + _min(SGLimits::max(), SGLimits::max(), SGLimits::max()), + _max(-SGLimits::max(), -SGLimits::max(), -SGLimits::max()) + { } + void setMin(const SGVec3& min) + { _min = min; } + const SGVec3& getMin() const + { return _min; } + + void setMax(const SGVec3& max) + { _max = max; } + const SGVec3& getMax() const + { return _max; } + + // Only works for floating point types + SGVec3 getCenter() const + { return T(0.5)*(_min + _max); } + + // Only valid for nonempty boxes + SGVec3 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::max(); + _min[1] = SGLimits::max(); + _min[2] = SGLimits::max(); + _max[0] = -SGLimits::max(); + _max[1] = -SGLimits::max(); + _max[2] = -SGLimits::max(); + } + + void expandBy(const SGVec3& v) + { _min = min(_min, v); _max = max(_max, v); } + + void expandBy(const SGBox& 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 _min; + SGVec3 _max; +}; + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGBox& box) +{ return s << "min = " << box.getMin() << ", max = " << box.getMax(); } + +#endif diff --git a/simgear/math/SGGeometry.hxx b/simgear/math/SGGeometry.hxx new file mode 100644 index 00000000..62d19ad9 --- /dev/null +++ b/simgear/math/SGGeometry.hxx @@ -0,0 +1,38 @@ +// 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 diff --git a/simgear/math/SGGeometryFwd.hxx b/simgear/math/SGGeometryFwd.hxx new file mode 100644 index 00000000..7d572306 --- /dev/null +++ b/simgear/math/SGGeometryFwd.hxx @@ -0,0 +1,51 @@ +// 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 +class SGBox; +typedef SGBox SGBoxf; +typedef SGBox SGBoxd; + +template +class SGSphere; +typedef SGSphere SGSpheref; +typedef SGSphere SGSphered; + +template +class SGRay; +typedef SGRay SGRayf; +typedef SGRay SGRayd; + +template +class SGLineSegment; +typedef SGLineSegment SGLineSegmentf; +typedef SGLineSegment SGLineSegmentd; + +template +class SGPlane; +typedef SGPlane SGPlanef; +typedef SGPlane SGPlaned; + +template +class SGTriangle; +typedef SGTriangle SGTrianglef; +typedef SGTriangle SGTriangled; + +#endif diff --git a/simgear/math/SGGeometryTest.cxx b/simgear/math/SGGeometryTest.cxx new file mode 100644 index 00000000..cd2383ee --- /dev/null +++ b/simgear/math/SGGeometryTest.cxx @@ -0,0 +1,442 @@ +// 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 +#endif + +#include +#include + +#include "SGGeometry.hxx" +#include "sg_random.h" + +template +SGVec3 rndVec3(void) +{ + return SGVec3(sg_random(), sg_random(), sg_random()); +} + +template +bool +TriangleLineIntersectionTest(void) +{ + unsigned nTests = 100000; + unsigned failedCount = 0; + for (unsigned i = 0; i < nTests; ++i) { + SGVec3 v0 = rndVec3(); + SGVec3 v1 = rndVec3(); + SGVec3 v2 = rndVec3(); + + SGTriangle 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 isectpt = v0 + u*(v1 - v0) + v*(v2 - v0); + + SGLineSegment lineSegment; + SGVec3 dir = rndVec3(); + + SGVec3 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 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 v0 = SGVec3(0, 0, 0); + SGVec3 v1 = SGVec3(1, 0, 0); + SGVec3 v2 = SGVec3(0, 1, 0); + + SGTriangle tri(v0, v1, v2); + + SGRay ray; + ray.set(SGVec3(0, 0, 1), SGVec3(0.1, 0.1, -1)); + if (!intersects(tri, ray)) { + std::cout << "Failed test #1!" << std::endl; + return false; + } + + ray.set(SGVec3(0, 0, 1), SGVec3(0, 0, -1)); + if (!intersects(tri, ray)) { + std::cout << "Failed test #2!" << std::endl; + return false; + } + + SGLineSegment lineSegment; + lineSegment.set(SGVec3(0, 0, 1), SGVec3(0.1, 0.1, -1)); + if (!intersects(tri, lineSegment)) { + std::cout << "Failed test #3!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(0, 0, -1)); + if (!intersects(tri, lineSegment)) { + std::cout << "Failed test #4!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(0, 1, -1)); + if (!intersects(tri, lineSegment)) { + std::cout << "Failed test #5!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(1, 0, -1)); + if (!intersects(tri, lineSegment)) { + std::cout << "Failed test #6!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(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(0, 0, 0), SGVec3(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(-1, 0, 0), SGVec3(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(-1, 1, 0), SGVec3(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(0, 0, 1), SGVec3(1, 1, -0.9)); + if (intersects(tri, lineSegment)) { + std::cout << "Failed test #11!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(0, -0.1, -1)); + if (intersects(tri, lineSegment)) { + std::cout << "Failed test #12!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(-0.1, -0.1, -1)); + if (intersects(tri, lineSegment)) { + std::cout << "Failed test #13!" << std::endl; + return false; + } + + lineSegment.set(SGVec3(0, 0, 1), SGVec3(-0.1, 0, -1)); + if (intersects(tri, lineSegment)) { + std::cout << "Failed test #14!" << std::endl; + return false; + } + + return true; +} + +template +bool +SphereLineIntersectionTest(void) +{ + unsigned nTests = 100000; + unsigned failedCount = 0; + for (unsigned i = 0; i < nTests; ++i) { + SGVec3 center = rndVec3(); + T radius = 2*sg_random(); + SGSphere sphere(center, radius); + + SGVec3 offset = normalize(rndVec3()); + T t = 4*sg_random(); + + // This one is the point we use to judge if the test should fail or not + SGVec3 base = center + t*offset; + + SGVec3 per = perpendicular(offset); + SGVec3 start = base + 4*sg_random()*per; + SGVec3 end = base - 4*sg_random()*per; + + SGLineSegment 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 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 center = rndVec3(); + T radius = 2*sg_random(); + SGSphere sphere(center, radius); + + SGVec3 offset = normalize(rndVec3()); + T t = 4*sg_random(); + + // This one is the point we use to judge if the test should fail or not + SGVec3 base = center + t*offset; + + SGVec3 start = base; + SGVec3 end = base + 2*sg_random()*offset; + + SGLineSegment 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 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 +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 box; + box.expandBy(rndVec3()); + box.expandBy(rndVec3()); + + SGVec3 center = box.getCenter(); + + // This one is the point we use to judge if the test should fail or not + SGVec3 base = rndVec3(); + SGVec3 dir = base - center; + + SGLineSegment 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 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()) + return EXIT_FAILURE; + if (!TriangleLineIntersectionTest()) + return EXIT_FAILURE; + + if (!SphereLineIntersectionTest()) + return EXIT_FAILURE; + if (!SphereLineIntersectionTest()) + return EXIT_FAILURE; + + if (!BoxLineIntersectionTest()) + return EXIT_FAILURE; + if (!BoxLineIntersectionTest()) + return EXIT_FAILURE; + + std::cout << "Successfully passed all tests!" << std::endl; + return EXIT_SUCCESS; +} diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx new file mode 100644 index 00000000..18a49e8d --- /dev/null +++ b/simgear/math/SGIntersect.hxx @@ -0,0 +1,595 @@ +// 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 +inline bool +intersects(const SGBox& box, const SGSphere& 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 +inline bool +intersects(const SGSphere& sphere, const SGBox& box) +{ return intersects(box, sphere); } + + +template +inline bool +intersects(const SGVec3& v, const SGBox& 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 +inline bool +intersects(const SGBox& box, const SGVec3& v) +{ return intersects(v, box); } + + +template +inline bool +intersects(const SGRay& ray, const SGPlane& 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::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::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 +inline bool +intersects(const SGPlane& plane, const SGRay& ray) +{ return intersects(ray, plane); } + +template +inline bool +intersects(SGVec3& dst, const SGRay& ray, const SGPlane& 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::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::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 +inline bool +intersects(SGVec3& dst, const SGPlane& plane, const SGRay& ray) +{ return intersects(dst, ray, plane); } + +template +inline bool +intersects(const SGLineSegment& lineSegment, const SGPlane& 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::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::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 +inline bool +intersects(const SGPlane& plane, const SGLineSegment& lineSegment) +{ return intersects(lineSegment, plane); } + +template +inline bool +intersects(SGVec3& dst, const SGLineSegment& lineSegment, const SGPlane& 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::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::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 +inline bool +intersects(SGVec3& dst, const SGPlane& plane, const SGLineSegment& lineSegment) +{ return intersects(dst, lineSegment, plane); } + + +template +inline bool +intersects(const SGRay& ray, const SGSphere& sphere) +{ + // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering, + // second edition, page 571 + SGVec3 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 +inline bool +intersects(const SGSphere& sphere, const SGRay& ray) +{ return intersects(ray, sphere); } + +template +inline bool +intersects(const SGLineSegment& lineSegment, const SGSphere& sphere) +{ + // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering, + // second edition, page 571 + SGVec3 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 +inline bool +intersects(const SGSphere& sphere, const SGLineSegment& lineSegment) +{ return intersects(lineSegment, sphere); } + + +template +inline bool +// FIXME do not use that default argument later. Just for development now +intersects(SGVec3& x, const SGTriangle& tri, const SGRay& 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 p = cross(ray.getDirection(), tri.getEdge(1)); + + T denom = dot(p, tri.getEdge(0)); + T signDenom = copysign(1, denom); + + SGVec3 s = ray.getOrigin() - tri.getBaseVertex(); + SGVec3 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::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 +inline bool +intersects(const SGTriangle& tri, const SGRay& ray, T eps = 0) +{ + // FIXME: for now just wrap the other method. When that has prooven + // well optimized, implement that special case + SGVec3 dummy; + return intersects(dummy, tri, ray, eps); +} + +template +inline bool +// FIXME do not use that default argument later. Just for development now +intersects(SGVec3& x, const SGTriangle& tri, const SGLineSegment& 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 p = cross(lineSegment.getDirection(), tri.getEdge(1)); + + T denom = dot(p, tri.getEdge(0)); + T signDenom = copysign(1, denom); + + SGVec3 s = lineSegment.getStart() - tri.getBaseVertex(); + SGVec3 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::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 +inline bool +intersects(const SGTriangle& tri, const SGLineSegment& lineSegment, T eps = 0) +{ + // FIXME: for now just wrap the othr method. When that has prooven + // well optimized, implement that special case + SGVec3 dummy; + return intersects(dummy, tri, lineSegment, eps); +} + + +template +inline bool +intersects(const SGVec3& v, const SGSphere& sphere) +{ + if (sphere.empty()) + return false; + return distSqr(v, sphere.getCenter()) <= sphere.getRadius2(); +} +template +inline bool +intersects(const SGSphere& sphere, const SGVec3& v) +{ return intersects(v, sphere); } + + +template +inline bool +intersects(const SGBox& box, const SGLineSegment& lineSegment) +{ + // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering + + SGVec3 c = lineSegment.getCenter() - box.getCenter(); + SGVec3 w = 0.5*lineSegment.getDirection(); + SGVec3 v(fabs(w.x()), fabs(w.y()), fabs(w.z())); + SGVec3 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 +inline bool +intersects(const SGLineSegment& lineSegment, const SGBox& box) +{ return intersects(box, lineSegment); } + +template +inline bool +intersects(const SGBox& box, const SGRay& 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::min()) { + if (cOrigin < cMin) + return false; + if (cMax < cOrigin) + return false; + } + + T near = - SGLimits::max(); + T far = SGLimits::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 +inline bool +intersects(const SGRay& ray, const SGBox& box) +{ return intersects(box, ray); } + +#endif diff --git a/simgear/math/SGLineSegment.hxx b/simgear/math/SGLineSegment.hxx new file mode 100644 index 00000000..71cdcc85 --- /dev/null +++ b/simgear/math/SGLineSegment.hxx @@ -0,0 +1,62 @@ +// 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 +class SGLineSegment { +public: + SGLineSegment() + { } + SGLineSegment(const SGVec3& start, const SGVec3& end) : + _start(start), + _direction(end - start) + { } + + void set(const SGVec3& start, const SGVec3& end) + { _start = start; _direction = end - start; } + + const SGVec3& getStart() const + { return _start; } + SGVec3 getEnd() const + { return _start + _direction; } + const SGVec3& getDirection() const + { return _direction; } + SGVec3 getNormalizedDirection() const + { return normalize(getDirection()); } + + SGVec3 getCenter() const + { return _start + T(0.5)*_direction; } + +private: + SGVec3 _start; + SGVec3 _direction; +}; + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, + const SGLineSegment& lineSegment) +{ + return s << "line segment: start = " << lineSegment.getStart() + << ", end = " << lineSegment.getEnd(); +} + +#endif diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx index 3f0d5cc6..acbdcb52 100644 --- a/simgear/math/SGMathTest.cxx +++ b/simgear/math/SGMathTest.cxx @@ -174,7 +174,9 @@ MatrixTest(void) // Create some test matrix SGVec3 v0(2, 7, 17); SGQuat q0 = SGQuat::fromAngleAxis(SGMisc::pi(), normalize(v0)); - SGMatrix m0(q0, v0); + SGMatrix m0; + m0.postMultTranslate(v0); + m0.postMultRotate(q0); // Check the tqo forms of the inverse for that kind of special matrix SGMatrix m1, m2; @@ -236,7 +238,9 @@ sgInterfaceTest(void) 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 @@ -283,7 +287,9 @@ sgdInterfaceTest(void) 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 diff --git a/simgear/math/SGPlane.hxx b/simgear/math/SGPlane.hxx new file mode 100644 index 00000000..e7755e0e --- /dev/null +++ b/simgear/math/SGPlane.hxx @@ -0,0 +1,59 @@ +// 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 +class SGPlane { +public: + SGPlane() + { } + SGPlane(const SGVec3& normal, T dist) : + _normal(normal), _dist(dist) + { } + SGPlane(const SGVec3 vertices[3]) : + _normal(normalize(cross(vertices[1] - vertices[0], + vertices[2] - vertices[0]))), + _dist(-dot(_normal, vertices[0])) + { } + + void setNormal(const SGVec3& normal) + { _normal = normal; } + const SGVec3& 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 _normal; + T _dist; +}; + +#endif diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index 2c3a0a7d..bb607885 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -130,13 +130,17 @@ public: 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::pi() - T(0.5)*lat; + T yd2 = T(-0.25)*SGMisc::pi() - T(0.5)*lat; T Szd2 = sin(zd2); T Syd2 = sin(yd2); T Czd2 = cos(zd2); @@ -147,17 +151,51 @@ public: 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::deg2rad(lon), SGMisc::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::pi(); + T zd2 = T(0.5)*lon + T(0.25)*SGMisc::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::deg2rad(lon), SGMisc::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& axis) { @@ -243,7 +281,7 @@ public: static SGQuat fromChangeSign(const SGVec3& 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)); diff --git a/simgear/math/SGRay.hxx b/simgear/math/SGRay.hxx new file mode 100644 index 00000000..8e2b5b35 --- /dev/null +++ b/simgear/math/SGRay.hxx @@ -0,0 +1,62 @@ +// 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 +class SGRay { +public: + SGRay() + { } + SGRay(const SGVec3& origin, const SGVec3& dir) : + _origin(origin), _direction(dir) + { } + + void set(const SGVec3& origin, const SGVec3& dir) + { _origin = origin; _direction = dir; } + + void setOrigin(const SGVec3& origin) + { _origin = origin; } + const SGVec3& getOrigin() const + { return _origin; } + + void setDirection(const SGVec3& direction) + { _direction = direction; } + const SGVec3& getDirection() const + { return _direction; } + + SGVec3 getNormalizedDirection() const + { return normalize(getDirection()); } + +private: + SGVec3 _origin; + SGVec3 _direction; +}; + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, + const SGRay& ray) +{ + return s << "ray: origin = " << ray.getOrigin() + << ", direction = " << ray.getDirection(); +} + +#endif diff --git a/simgear/math/SGSphere.hxx b/simgear/math/SGSphere.hxx new file mode 100644 index 00000000..792d939b --- /dev/null +++ b/simgear/math/SGSphere.hxx @@ -0,0 +1,87 @@ +// 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 +class SGSphere { +public: + SGSphere() : + _radius(-1) + { } + SGSphere(const SGVec3& center, const T& radius) : + _center(center), + _radius(radius) + { } + + const SGVec3& getCenter() const + { return _center; } + void setCenter(const SGVec3& 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& 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 _center; + T _radius; +}; + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, + const SGSphere& sphere) +{ + return s << "center = " << sphere.getCenter() + << ", radius = " << sphere.getRadius(); +} + +#endif diff --git a/simgear/math/SGTriangle.hxx b/simgear/math/SGTriangle.hxx new file mode 100644 index 00000000..888102cf --- /dev/null +++ b/simgear/math/SGTriangle.hxx @@ -0,0 +1,101 @@ +// 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 +class SGTriangle { +public: + SGTriangle() + { } + SGTriangle(const SGVec3& v0, const SGVec3& v1, const SGVec3& v2) + { set(v0, v1, v2); } + SGTriangle(const SGVec3 v[3]) + { set(v); } + + void set(const SGVec3& v0, const SGVec3& v1, const SGVec3& v2) + { + _v0 = v0; + _d[0] = v1 - v0; + _d[1] = v2 - v0; + } + void set(const SGVec3 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 getVertex(unsigned i) const + { + if (0 < i) + return _v0 + _d[i-1]; + return _v0; + } + /// return the normalized surface normal + SGVec3 getNormal() const + { return normalize(cross(_d[0], _d[1])); } + + const SGVec3& getBaseVertex() const + { return _v0; } + void setBaseVertex(const SGVec3& v) + { _v0 = v; } + const SGVec3& getEdge(unsigned i) const + { return _d[i]; } + void setEdge(unsigned i, const SGVec3& d) + { _d[i] = d; } + + // flip the positive side + void flip() + { + SGVec3 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 _v0; + SGVec3 _d[2]; +}; + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, + const SGTriangle& triangle) +{ + return s << "triangle: v0 = " << triangle.getVertex(0) + << ", v1 = " << triangle.getVertex(1) + << ", v2 = " << triangle.getVertex(2); +} + +#endif diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 7d4eec13..e33e8c05 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -364,18 +364,29 @@ cross(const SGVec3& v1, const SGVec3& v2) v1(0)*v2(1) - v1(1)*v2(0)); } -/// return any vector perpendicular to v +/// return any normalized vector perpendicular to v template inline SGVec3 perpendicular(const SGVec3& 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(quot, -1, 0); + } else if (absv1 < absv2 && absv3 < absv2) { + T quot = v(2)/v(1); + return (1/sqrt(1+quot*quot))*SGVec3(0, quot, -1); + } else if (absv1 < absv3 && absv2 < absv3) { + T quot = v(0)/v(2); + return (1/sqrt(1+quot*quot))*SGVec3(-1, 0, quot); + } else { + // the all zero case ... + return SGVec3(0, 0, 0); + } } /// The euclidean norm of the vector, that is what most people call length -- 2.39.5