FGFS-4.1/simgear
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Modified Files:

Browse Source 
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.
This commit is contained in:
frohlich
2006-12-27 09:23:39 +00:00
parent 3b83487611
commit de020ee695
14 changed files with 1693 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
simgear/math/Makefile.am
View File

@@ -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 \

107
simgear/math/SGBox.hxx Normal file
View File

@@ -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<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

38
simgear/math/SGGeometry.hxx Normal file
View File

@@ -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

51
simgear/math/SGGeometryFwd.hxx Normal file
View File

@@ -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<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

442
simgear/math/SGGeometryTest.cxx Normal file
View File

@@ -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 <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;
}

595
simgear/math/SGIntersect.hxx Normal file
View File

@@ -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<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

62
simgear/math/SGLineSegment.hxx Normal file
View File

@@ -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<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

12
simgear/math/SGMathTest.cxx
View File

@@ -174,7 +174,9 @@ MatrixTest(void)
// 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;
@@ -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

59
simgear/math/SGPlane.hxx Normal file
View File

@@ -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<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

58
simgear/math/SGQuat.hxx
View File

@@ -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<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);
@@ -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<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)
{
@@ -243,7 +281,7 @@ public:
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));

62
simgear/math/SGRay.hxx Normal file
View File

@@ -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<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

87
simgear/math/SGSphere.hxx Normal file
View File

@@ -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<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

101
simgear/math/SGTriangle.hxx Normal file
View File

@@ -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<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

25
simgear/math/SGVec3.hxx
View File

@@ -364,18 +364,29 @@ cross(const SGVec3<T>& v1, const SGVec3<T>& v2)
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