simgear/bvh/BVHNearestPointVisitor.hxx

   1 // Copyright (C) 2008 - 2009  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef BVHNearestPointVisitor_hxx
  19 #define BVHNearestPointVisitor_hxx
  20
  21 #include <simgear/math/SGGeometry.hxx>
  22
  23 #include "BVHVisitor.hxx"
  24
  25 #include "BVHNode.hxx"
  26 #include "BVHGroup.hxx"
  27 #include "BVHPageNode.hxx"
  28 #include "BVHTransform.hxx"
  29 #include "BVHLineGeometry.hxx"
  30 #include "BVHStaticGeometry.hxx"
  31
  32 #include "BVHStaticData.hxx"
  33
  34 #include "BVHStaticNode.hxx"
  35 #include "BVHStaticTriangle.hxx"
  36 #include "BVHStaticBinary.hxx"
  37
  38 namespace simgear {
  39
  40 class BVHNearestPointVisitor : public BVHVisitor {
  41 public:
  42     BVHNearestPointVisitor(const SGSphered& sphere, const double& t) :
  43         _sphere(sphere),
  44         _time(t),
  45         _material(0),
  46         _id(0),
  47         _havePoint(false)
  48     { }
  49
  50     virtual void apply(BVHGroup& leaf)
  51     {
  52         if (!intersects(_sphere, leaf.getBoundingSphere()))
  53             return;
  54         leaf.traverse(*this);
  55     }
  56     virtual void apply(BVHPageNode& leaf)
  57     {
  58         if (!intersects(_sphere, leaf.getBoundingSphere()))
  59             return;
  60         leaf.traverse(*this);
  61     }
  62     virtual void apply(BVHTransform& transform)
  63     {
  64         if (!intersects(_sphere, transform.getBoundingSphere()))
  65             return;
  66
  67         SGSphered sphere = _sphere;
  68         _sphere = transform.sphereToLocal(sphere);
  69         bool havePoint = _havePoint;
  70         _havePoint = false;
  71
  72         transform.traverse(*this);
  73
  74         if (_havePoint) {
  75             _point = transform.ptToWorld(_point);
  76             _linearVelocity = transform.vecToWorld(_linearVelocity);
  77             _angularVelocity = transform.vecToWorld(_angularVelocity);
  78         }
  79         _havePoint |= havePoint;
  80         _sphere.setCenter(sphere.getCenter());
  81     }
  82     virtual void apply(BVHMotionTransform& transform)
  83     {
  84         if (!intersects(_sphere, transform.getBoundingSphere()))
  85             return;
  86
  87         SGSphered sphere = _sphere;
  88         _sphere = transform.sphereToLocal(sphere, _time);
  89         bool havePoint = _havePoint;
  90         _havePoint = false;
  91
  92         transform.traverse(*this);
  93
  94         if (_havePoint) {
  95             SGMatrixd toWorld = transform.getToWorldTransform(_time);
  96             SGVec3d localCenter = _sphere.getCenter();
  97             _linearVelocity += transform.getLinearVelocityAt(localCenter);
  98             _angularVelocity += transform.getAngularVelocity();
  99             _linearVelocity = toWorld.xformVec(_linearVelocity);
 100             _angularVelocity = toWorld.xformVec(_angularVelocity);
 101             _point = toWorld.xformPt(_point);
 102             if (!_id)
 103                 _id = transform.getId();
 104         }
 105         _havePoint |= havePoint;
 106         _sphere.setCenter(sphere.getCenter());
 107     }
 108     virtual void apply(BVHLineGeometry& node)
 109     { }
 110     virtual void apply(BVHStaticGeometry& node)
 111     {
 112         if (!intersects(_sphere, node.getBoundingSphere()))
 113             return;
 114         node.traverse(*this);
 115     }
 116
 117     virtual void apply(const BVHStaticBinary& node, const BVHStaticData& data)
 118     {
 119         if (!intersects(_sphere, node.getBoundingBox()))
 120             return;
 121         node.traverse(*this, data, _sphere.getCenter());
 122     }
 123     virtual void apply(const BVHStaticTriangle& node, const BVHStaticData& data)
 124     {
 125         SGVec3f center(_sphere.getCenter());
 126         SGVec3d closest(closestPoint(node.getTriangle(data), center));
 127         if (!intersects(_sphere, closest))
 128             return;
 129         _point = closest;
 130         _linearVelocity = SGVec3d::zeros();
 131         _angularVelocity = SGVec3d::zeros();
 132         _material = data.getMaterial(node.getMaterialIndex());
 133         // The trick is to decrease the radius of the search sphere.
 134         _sphere.setRadius(length(closest - _sphere.getCenter()));
 135         _havePoint = true;
 136         _id = 0;
 137     }
 138
 139     void setSphere(const SGSphered& sphere)
 140     { _sphere = sphere; }
 141     const SGSphered& getSphere() const
 142     { return _sphere; }
 143
 144     const SGVec3d& getPoint() const
 145     { return _point; }
 146     const SGVec3d& getLinearVelocity() const
 147     { return _linearVelocity; }
 148     const SGVec3d& getAngularVelocity() const
 149     { return _angularVelocity; }
 150     const BVHMaterial* getMaterial() const
 151     { return _material; }
 152     BVHNode::Id getId() const
 153     { return _id; }
 154
 155     bool empty() const
 156     { return !_havePoint; }
 157
 158 private:
 159     SGSphered _sphere;
 160     double _time;
 161
 162     SGVec3d _point;
 163     SGVec3d _linearVelocity;
 164     SGVec3d _angularVelocity;
 165     const BVHMaterial* _material;
 166     BVHNode::Id _id;
 167
 168     bool _havePoint;
 169 };
 170
 171 }
 172
 173 #endif