From: frohlich <frohlich>
Date: Sun, 22 Feb 2009 08:13:24 +0000 (+0000)
Subject: Small cleanups, bug fixes and improovements to the geometry/math stuff.
X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=d11954e80c9e62f87da087b29f6bfb7a6d7e84b5;p=simgear.git

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

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<T>::max(), SGLimits<T>::max(), SGLimits<T>::max()),
     _max(-SGLimits<T>::max(), -SGLimits<T>::max(), -SGLimits<T>::max())
   { }
+  SGBox(const SGVec3<T>& pt) :
+    _min(pt),
+    _max(pt)
+  { }
+  SGBox(const SGVec3<T>& min, const SGVec3<T>& max) :
+    _min(min),
+    _max(max)
+  { }
+  template<typename S>
+  explicit SGBox(const SGBox<S>& box) :
+    _min(box.getMin()),
+    _max(box.getMax())
+  { }
+
   void setMin(const SGVec3<T>& min)
   { _min = min; }
   const SGVec3<T>& getMin() const
@@ -35,6 +49,29 @@ public:
   const SGVec3<T>& getMax() const
   { return _max; }
 
+  SGVec3<T> getCorner(unsigned i) const
+  { return SGVec3<T>((i&1) ? _min[0] : _max[0],
+                     (i&2) ? _min[1] : _max[1],
+                     (i&4) ? _min[2] : _max[2]); }
+
+  template<typename S>
+  SGVec3<T> getNearestCorner(const SGVec3<S>& pt) const
+  {
+    SGVec3<T> center = getCenter();
+    return SGVec3<T>((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<typename S>
+  SGVec3<T> getFarestCorner(const SGVec3<S>& pt) const
+  {
+    SGVec3<T> center = getCenter();
+    return SGVec3<T>((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<T> getCenter() const
   { return T(0.5)*(_min + _max); }
@@ -42,6 +79,8 @@ public:
   // Only valid for nonempty boxes
   SGVec3<T> getSize() const
   { return _max - _min; }
+  SGVec3<T> 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<T> 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<T> 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;
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<typename T>
 inline bool
-intersects(const SGBox<T>& box, const SGSphere<T>& sphere)
+intersects(const SGSphere<T>& s1, const SGSphere<T>& s2)
+{
+  if (s1.empty())
+    return false;
+  if (s2.empty())
+    return false;
+
+  T dist = s1.getRadius() + s2.getRadius();
+  return distSqr(s1.getCenter(), s2.getCenter()) <= dist*dist;
+}
+
+
+template<typename T1, typename T2>
+inline bool
+intersects(const SGBox<T1>& box, const SGSphere<T2>& sphere)
 {
   if (sphere.empty())
     return false;
@@ -45,15 +59,15 @@ intersects(const SGBox<T>& box, const SGSphere<T>& sphere)
   return true;
 }
 // make it symmetric
-template<typename T>
+template<typename T1, typename T2>
 inline bool
-intersects(const SGSphere<T>& sphere, const SGBox<T>& box)
+intersects(const SGSphere<T1>& sphere, const SGBox<T2>& box)
 { return intersects(box, sphere); }
 
 
-template<typename T>
+template<typename T1, typename T2>
 inline bool
-intersects(const SGVec3<T>& v, const SGBox<T>& box)
+intersects(const SGVec3<T1>& v, const SGBox<T2>& box)
 {
   if (v[0] < box.getMin()[0])
     return false;
@@ -69,9 +83,9 @@ intersects(const SGVec3<T>& v, const SGBox<T>& box)
     return false;
   return true;
 }
-template<typename T>
+template<typename T1, typename T2>
 inline bool
-intersects(const SGBox<T>& box, const SGVec3<T>& v)
+intersects(const SGBox<T1>& box, const SGVec3<T2>& v)
 { return intersects(v, box); }
 
 
@@ -529,7 +543,7 @@ 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
+  // FIXME: for now just wrap the other method. When that has prooven
   // well optimized, implement that special case
   SGVec3<T> dummy;
   return intersects(dummy, tri, lineSegment, eps);
@@ -557,9 +571,9 @@ 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> w = T(0.5)*lineSegment.getDirection();
   SGVec3<T> v(fabs(w.x()), fabs(w.y()), fabs(w.z()));
-  SGVec3<T> h = 0.5*box.getSize();
+  SGVec3<T> h = T(0.5)*box.getSize();
 
   if (fabs(c[0]) > v[0] + h[0])
     return false;
@@ -624,4 +638,26 @@ inline bool
 intersects(const SGRay<T>& ray, const SGBox<T>& box)
 { return intersects(box, ray); }
 
+template<typename T1, typename T2>
+inline bool
+intersects(const SGBox<T1>& box1, const SGBox<T2>& box2)
+{
+  if (box2.getMax()[0] < box1.getMin()[0])
+    return false;
+  if (box1.getMax()[0] < box2.getMin()[0])
+    return false;
+
+  if (box2.getMax()[1] < box1.getMin()[1])
+    return false;
+  if (box1.getMax()[1] < box2.getMin()[1])
+    return false;
+
+  if (box2.getMax()[2] < box1.getMin()[2])
+    return false;
+  if (box1.getMax()[2] < box2.getMin()[2])
+    return false;
+
+  return true;
+}
+
 #endif
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<typename S>
+  explicit SGLineSegment(const SGLineSegment<S>& lineSegment) :
+    _start(lineSegment.getStart()),
+    _direction(lineSegment.getDirection())
+  { }
 
   void set(const SGVec3<T>& start, const SGVec3<T>& 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<typename S>
   SGMatrix& preMultTranslate(const SGVec3<S>& t)
   {
-    for (unsigned i = 0; i < SGMatrix<T>::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<typename S>
   SGMatrix& postMultTranslate(const SGVec3<S>& t)
   {
     SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
-      SGVec4<T> tmp((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
+      SGVec4<T> 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<T>& normal, T dist) :
+  SGPlane(const SGVec3<T>& normal, const T& dist) :
     _normal(normal), _dist(dist)
   { }
+  SGPlane(const SGVec3<T>& normal, const SGVec3<T>& point) :
+    _normal(normal), _dist(-dot(normal, point))
+  { }
   SGPlane(const SGVec3<T> vertices[3]) :
     _normal(normalize(cross(vertices[1] - vertices[0],
                             vertices[2] - vertices[0]))),
     _dist(-dot(_normal, vertices[0]))
   { }
+  SGPlane(const SGVec3<T>& v0, const SGVec3<T>& v1, const SGVec3<T>& v2) :
+    _normal(normalize(cross(v1 - v0, v2 - v0))),
+    _dist(-dot(_normal, v0))
+  { }
+  template<typename S>
+  explicit SGPlane(const SGPlane<S>& plane) :
+    _normal(plane.getNormal()), _dist(plane.getDist())
+  { }
 
   void setNormal(const SGVec3<T>& normal)
   { _normal = normal; }
@@ -42,6 +53,10 @@ public:
   const T& getDist() const
   { return _dist; }
 
+  /// Return a point on the plane 
+  SGVec3<T> 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<T>& origin, const SGVec3<T>& dir) :
     _origin(origin), _direction(dir)
   { }
+  template<typename S>
+  explicit SGRay(const SGRay<S>& ray) :
+    _origin(ray.getOrigin()), _direction(ray.getDirection())
+  { }
 
   void set(const SGVec3<T>& origin, const SGVec3<T>& 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<typename S>
+  explicit SGSphere(const SGSphere<S>& sphere) :
+    _center(sphere.getCenter()),
+    _radius(sphere.getRadius())
+  { }
 
   const SGVec3<T>& 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<T>& s)
+  {
+    if (s.empty())
+      return;
+
+    if (empty()) {
+      _center = s.getCenter();
+      _radius = s.getRadius();
+      return;
+    }
+
+    T dist = length(_center - s.getCenter());
+    if (dist <= SGLimits<T>::min()) {
+      _radius = SGMisc<T>::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<T>& box)
+  {
+    if (box.empty())
+      return;
+
+    if (empty()) {
+      _center = box.getCenter();
+      _radius = T(0.5)*length(box.getSize());
+      return;
+    }
+
+    SGVec3<T> boxCenter = box.getCenter();
+    SGVec3<T> 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<T> _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<typename S>
+  explicit SGVec2(const SGVec2<S>& 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<typename S>
+  explicit SGVec3(const SGVec3<S>& 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<typename S>
+  explicit SGVec4(const SGVec4<S>& 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)