From d11954e80c9e62f87da087b29f6bfb7a6d7e84b5 Mon Sep 17 00:00:00 2001 From: frohlich Date: Sun, 22 Feb 2009 08:13:24 +0000 Subject: [PATCH] Small cleanups, bug fixes and improovements to the geometry/math stuff. Modified Files: simgear/math/SGBox.hxx simgear/math/SGIntersect.hxx simgear/math/SGLineSegment.hxx simgear/math/SGMatrix.hxx simgear/math/SGPlane.hxx simgear/math/SGRay.hxx simgear/math/SGSphere.hxx simgear/math/SGVec2.hxx simgear/math/SGVec3.hxx simgear/math/SGVec4.hxx --- simgear/math/SGBox.hxx | 53 ++++++++++++++++++++++++++- simgear/math/SGIntersect.hxx | 56 +++++++++++++++++++++++------ simgear/math/SGLineSegment.hxx | 5 +++ simgear/math/SGMatrix.hxx | 14 +++++--- simgear/math/SGPlane.hxx | 17 ++++++++- simgear/math/SGRay.hxx | 4 +++ simgear/math/SGSphere.hxx | 66 +++++++++++++++++++++++++++++++++- simgear/math/SGVec2.hxx | 3 ++ simgear/math/SGVec3.hxx | 3 ++ simgear/math/SGVec4.hxx | 3 ++ 10 files changed, 207 insertions(+), 17 deletions(-) diff --git a/simgear/math/SGBox.hxx b/simgear/math/SGBox.hxx index 272c7cab..8024a131 100644 --- a/simgear/math/SGBox.hxx +++ b/simgear/math/SGBox.hxx @@ -25,6 +25,20 @@ public: _min(SGLimits::max(), SGLimits::max(), SGLimits::max()), _max(-SGLimits::max(), -SGLimits::max(), -SGLimits::max()) { } + SGBox(const SGVec3& pt) : + _min(pt), + _max(pt) + { } + SGBox(const SGVec3& min, const SGVec3& max) : + _min(min), + _max(max) + { } + template + explicit SGBox(const SGBox& box) : + _min(box.getMin()), + _max(box.getMax()) + { } + void setMin(const SGVec3& min) { _min = min; } const SGVec3& getMin() const @@ -35,6 +49,29 @@ public: const SGVec3& getMax() const { return _max; } + SGVec3 getCorner(unsigned i) const + { return SGVec3((i&1) ? _min[0] : _max[0], + (i&2) ? _min[1] : _max[1], + (i&4) ? _min[2] : _max[2]); } + + template + SGVec3 getNearestCorner(const SGVec3& pt) const + { + SGVec3 center = getCenter(); + return SGVec3((pt[0] <= center[0]) ? _min[0] : _max[0], + (pt[1] <= center[1]) ? _min[1] : _max[1], + (pt[2] <= center[2]) ? _min[2] : _max[2]); + } + template + SGVec3 getFarestCorner(const SGVec3& pt) const + { + SGVec3 center = getCenter(); + return SGVec3((pt[0] > center[0]) ? _min[0] : _max[0], + (pt[1] > center[1]) ? _min[1] : _max[1], + (pt[2] > center[2]) ? _min[2] : _max[2]); + } + + // Only works for floating point types SGVec3 getCenter() const { return T(0.5)*(_min + _max); } @@ -42,6 +79,8 @@ public: // Only valid for nonempty boxes SGVec3 getSize() const { return _max - _min; } + SGVec3 getHalfSize() const + { return T(0.5)*getSize(); } T getVolume() const { @@ -83,7 +122,7 @@ public: // Note that this only works if the box is nonmepty unsigned getBroadestAxis() const { - SGVec3d size = getSize(); + SGVec3 size = getSize(); if (size[1] <= size[0] && size[2] <= size[0]) return 0; else if (size[2] <= size[1]) @@ -92,6 +131,18 @@ public: return 2; } + // Note that this only works if the box is nonmepty + unsigned getSmallestAxis() const + { + SGVec3 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; diff --git a/simgear/math/SGIntersect.hxx b/simgear/math/SGIntersect.hxx index 5951e158..bdc350f2 100644 --- a/simgear/math/SGIntersect.hxx +++ b/simgear/math/SGIntersect.hxx @@ -20,7 +20,21 @@ template inline bool -intersects(const SGBox& box, const SGSphere& sphere) +intersects(const SGSphere& s1, const SGSphere& s2) +{ + if (s1.empty()) + return false; + if (s2.empty()) + return false; + + T dist = s1.getRadius() + s2.getRadius(); + return distSqr(s1.getCenter(), s2.getCenter()) <= dist*dist; +} + + +template +inline bool +intersects(const SGBox& box, const SGSphere& sphere) { if (sphere.empty()) return false; @@ -45,15 +59,15 @@ intersects(const SGBox& box, const SGSphere& sphere) return true; } // make it symmetric -template +template inline bool -intersects(const SGSphere& sphere, const SGBox& box) +intersects(const SGSphere& sphere, const SGBox& box) { return intersects(box, sphere); } -template +template inline bool -intersects(const SGVec3& v, const SGBox& box) +intersects(const SGVec3& v, const SGBox& box) { if (v[0] < box.getMin()[0]) return false; @@ -69,9 +83,9 @@ intersects(const SGVec3& v, const SGBox& box) return false; return true; } -template +template inline bool -intersects(const SGBox& box, const SGVec3& v) +intersects(const SGBox& box, const SGVec3& v) { return intersects(v, box); } @@ -529,7 +543,7 @@ 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 + // FIXME: for now just wrap the other method. When that has prooven // well optimized, implement that special case SGVec3 dummy; return intersects(dummy, tri, lineSegment, eps); @@ -557,9 +571,9 @@ 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 w = T(0.5)*lineSegment.getDirection(); SGVec3 v(fabs(w.x()), fabs(w.y()), fabs(w.z())); - SGVec3 h = 0.5*box.getSize(); + SGVec3 h = T(0.5)*box.getSize(); if (fabs(c[0]) > v[0] + h[0]) return false; @@ -624,4 +638,26 @@ inline bool intersects(const SGRay& ray, const SGBox& box) { return intersects(box, ray); } +template +inline bool +intersects(const SGBox& box1, const SGBox& box2) +{ + if (box2.getMax()[0] < box1.getMin()[0]) + return false; + if (box1.getMax()[0] < box2.getMin()[0]) + return false; + + if (box2.getMax()[1] < box1.getMin()[1]) + return false; + if (box1.getMax()[1] < box2.getMin()[1]) + return false; + + if (box2.getMax()[2] < box1.getMin()[2]) + return false; + if (box1.getMax()[2] < box2.getMin()[2]) + return false; + + return true; +} + #endif diff --git a/simgear/math/SGLineSegment.hxx b/simgear/math/SGLineSegment.hxx index 71cdcc85..0e509621 100644 --- a/simgear/math/SGLineSegment.hxx +++ b/simgear/math/SGLineSegment.hxx @@ -27,6 +27,11 @@ public: _start(start), _direction(end - start) { } + template + explicit SGLineSegment(const SGLineSegment& lineSegment) : + _start(lineSegment.getStart()), + _direction(lineSegment.getDirection()) + { } void set(const SGVec3& start, const SGVec3& end) { _start = start; _direction = end - start; } diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx index e91140d2..ad53dcd8 100644 --- a/simgear/math/SGMatrix.hxx +++ b/simgear/math/SGMatrix.hxx @@ -184,16 +184,22 @@ public: template SGMatrix& preMultTranslate(const SGVec3& t) { - for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) - (*this)(i,3) += T(t(i)); - return *this; + for (unsigned i = 0; i < 3; ++i) { + T tmp = T(t(i)); + if (tmp == 0) + continue; + (*this)(i,0) += tmp*(*this)(3,0); + (*this)(i,1) += tmp*(*this)(3,1); + (*this)(i,2) += tmp*(*this)(3,2); + (*this)(i,3) += tmp*(*this)(3,3); + } } template SGMatrix& postMultTranslate(const SGVec3& t) { SGVec4 col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3)); for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) { - SGVec4 tmp((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3)); + SGVec4 tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i)); col3 += T(t(i))*tmp; } (*this)(0,3) = col3(0); (*this)(1,3) = col3(1); diff --git a/simgear/math/SGPlane.hxx b/simgear/math/SGPlane.hxx index e7755e0e..971e72c8 100644 --- a/simgear/math/SGPlane.hxx +++ b/simgear/math/SGPlane.hxx @@ -23,14 +23,25 @@ class SGPlane { public: SGPlane() { } - SGPlane(const SGVec3& normal, T dist) : + SGPlane(const SGVec3& normal, const T& dist) : _normal(normal), _dist(dist) { } + SGPlane(const SGVec3& normal, const SGVec3& point) : + _normal(normal), _dist(-dot(normal, point)) + { } SGPlane(const SGVec3 vertices[3]) : _normal(normalize(cross(vertices[1] - vertices[0], vertices[2] - vertices[0]))), _dist(-dot(_normal, vertices[0])) { } + SGPlane(const SGVec3& v0, const SGVec3& v1, const SGVec3& v2) : + _normal(normalize(cross(v1 - v0, v2 - v0))), + _dist(-dot(_normal, v0)) + { } + template + explicit SGPlane(const SGPlane& plane) : + _normal(plane.getNormal()), _dist(plane.getDist()) + { } void setNormal(const SGVec3& normal) { _normal = normal; } @@ -42,6 +53,10 @@ public: const T& getDist() const { return _dist; } + /// Return a point on the plane + SGVec3 getPointOnPlane() const + { return -_dist*_normal; } + /// That is the distance where we measure positive in direction of the normal T getPositiveDist() const { return -_dist; } diff --git a/simgear/math/SGRay.hxx b/simgear/math/SGRay.hxx index 8e2b5b35..afd7b041 100644 --- a/simgear/math/SGRay.hxx +++ b/simgear/math/SGRay.hxx @@ -26,6 +26,10 @@ public: SGRay(const SGVec3& origin, const SGVec3& dir) : _origin(origin), _direction(dir) { } + template + explicit SGRay(const SGRay& ray) : + _origin(ray.getOrigin()), _direction(ray.getDirection()) + { } void set(const SGVec3& origin, const SGVec3& dir) { _origin = origin; _direction = dir; } diff --git a/simgear/math/SGSphere.hxx b/simgear/math/SGSphere.hxx index 792d939b..7d8b3ab1 100644 --- a/simgear/math/SGSphere.hxx +++ b/simgear/math/SGSphere.hxx @@ -28,6 +28,11 @@ public: _center(center), _radius(radius) { } + template + explicit SGSphere(const SGSphere& sphere) : + _center(sphere.getCenter()), + _radius(sphere.getRadius()) + { } const SGVec3& getCenter() const { return _center; } @@ -41,7 +46,7 @@ public: T getRadius2() const { return _radius*_radius; } - const bool empty() const + bool empty() const { return !valid(); } bool valid() const @@ -68,6 +73,65 @@ public: _radius = newRadius; } + void expandBy(const SGSphere& s) + { + if (s.empty()) + return; + + if (empty()) { + _center = s.getCenter(); + _radius = s.getRadius(); + return; + } + + T dist = length(_center - s.getCenter()); + if (dist <= SGLimits::min()) { + _radius = SGMisc::max(_radius, s._radius); + return; + } + + // already included + if (dist + s.getRadius() <= _radius) + return; + + // new one includes all + if (dist + _radius <= s.getRadius()) { + _center = s.getCenter(); + _radius = s.getRadius(); + return; + } + + T newRadius = T(0.5)*(_radius + dist + s.getRadius()); + T ratio = (newRadius - _radius) / dist; + _radius = newRadius; + + _center[0] += ratio*(s._center[0] - _center[0]); + _center[1] += ratio*(s._center[1] - _center[1]); + _center[2] += ratio*(s._center[2] - _center[2]); + } + + void expandBy(const SGBox& box) + { + if (box.empty()) + return; + + if (empty()) { + _center = box.getCenter(); + _radius = T(0.5)*length(box.getSize()); + return; + } + + SGVec3 boxCenter = box.getCenter(); + SGVec3 corner; + for (unsigned i = 0; i < 3; ++i) { + if (_center[i] < boxCenter[i]) + corner[i] = box.getMax()[i]; + else + corner[i] = box.getMin()[i]; + } + expandBy(corner); + } + private: SGVec3 _center; T _radius; diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx index 0409ee44..f9104991 100644 --- a/simgear/math/SGVec2.hxx +++ b/simgear/math/SGVec2.hxx @@ -96,6 +96,9 @@ public: /// make sure it has at least 2 elements explicit SGVec2(const T* d) { data()[0] = d[0]; data()[1] = d[1]; } + template + explicit SGVec2(const SGVec2& d) + { data()[0] = d[0]; data()[1] = d[1]; } explicit SGVec2(const osg::Vec2f& d) { data()[0] = d[0]; data()[1] = d[1]; } explicit SGVec2(const osg::Vec2d& d) diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 76958199..4ec4145a 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -92,6 +92,9 @@ public: /// make sure it has at least 3 elements explicit SGVec3(const T* d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; } + template + explicit SGVec3(const SGVec3& d) + { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; } explicit SGVec3(const osg::Vec3f& d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; } explicit SGVec3(const osg::Vec3d& d) diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx index 13cacc9d..63309495 100644 --- a/simgear/math/SGVec4.hxx +++ b/simgear/math/SGVec4.hxx @@ -92,6 +92,9 @@ public: /// make sure it has at least 3 elements explicit SGVec4(const T* d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } + template + explicit SGVec4(const SGVec4& d) + { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } explicit SGVec4(const osg::Vec4f& d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } explicit SGVec4(const osg::Vec4d& d) -- 2.39.5