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 "BVHTransform.hxx"
  28 #include "BVHLineGeometry.hxx"
  29 #include "BVHStaticGeometry.hxx"
  30
  31 #include "BVHStaticData.hxx"
  32
  33 #include "BVHStaticNode.hxx"
  34 #include "BVHStaticTriangle.hxx"
  35 #include "BVHStaticBinary.hxx"
  36
  37 namespace simgear {
  38
  39 class BVHNearestPointVisitor : public BVHVisitor {
  40 public:
  41     BVHNearestPointVisitor(const SGSphered& sphere, const double& t) :
  42         _sphere(sphere),
  43         _time(t),
  44         _material(0),
  45         _id(0),
  46         _havePoint(false)
  47     { }
  48
  49     virtual void apply(BVHGroup& leaf)
  50     {
  51         if (!intersects(_sphere, leaf.getBoundingSphere()))
  52             return;
  53         leaf.traverse(*this);
  54     }
  55     virtual void apply(BVHTransform& transform)
  56     {
  57         if (!intersects(_sphere, transform.getBoundingSphere()))
  58             return;
  59
  60         SGSphered sphere = _sphere;
  61         _sphere = transform.sphereToLocal(sphere);
  62         bool havePoint = _havePoint;
  63         _havePoint = false;
  64
  65         transform.traverse(*this);
  66
  67         if (_havePoint) {
  68             _point = transform.ptToWorld(_point);
  69             _linearVelocity = transform.vecToWorld(_linearVelocity);
  70             _angularVelocity = transform.vecToWorld(_angularVelocity);
  71         }
  72         _havePoint |= havePoint;
  73         _sphere.setCenter(sphere.getCenter());
  74     }
  75     virtual void apply(BVHMotionTransform& transform)
  76     {
  77         if (!intersects(_sphere, transform.getBoundingSphere()))
  78             return;
  79
  80         SGSphered sphere = _sphere;
  81         _sphere = transform.sphereToLocal(sphere, _time);
  82         bool havePoint = _havePoint;
  83         _havePoint = false;
  84
  85         transform.traverse(*this);
  86
  87         if (_havePoint) {
  88             SGMatrixd toWorld = transform.getToWorldTransform(_time);
  89             SGVec3d localCenter = _sphere.getCenter();
  90             _linearVelocity += transform.getLinearVelocityAt(localCenter);
  91             _angularVelocity += transform.getAngularVelocity();
  92             _linearVelocity = toWorld.xformVec(_linearVelocity);
  93             _angularVelocity = toWorld.xformVec(_angularVelocity);
  94             _point = toWorld.xformPt(_point);
  95             if (!_id)
  96                 _id = transform.getId();
  97         }
  98         _havePoint |= havePoint;
  99         _sphere.setCenter(sphere.getCenter());
 100     }
 101     virtual void apply(BVHLineGeometry& node)
 102     { }
 103     virtual void apply(BVHStaticGeometry& node)
 104     {
 105         if (!intersects(_sphere, node.getBoundingSphere()))
 106             return;
 107         node.traverse(*this);
 108     }
 109
 110     virtual void apply(const BVHStaticBinary& node, const BVHStaticData& data)
 111     {
 112         if (!intersects(_sphere, node.getBoundingBox()))
 113             return;
 114         node.traverse(*this, data, _sphere.getCenter());
 115     }
 116     virtual void apply(const BVHStaticTriangle& node, const BVHStaticData& data)
 117     {
 118         SGVec3f center(_sphere.getCenter());
 119         SGVec3d closest(closestPoint(node.getTriangle(data), center));
 120         if (!intersects(_sphere, closest))
 121             return;
 122         _point = closest;
 123         _linearVelocity = SGVec3d::zeros();
 124         _angularVelocity = SGVec3d::zeros();
 125         _material = data.getMaterial(node.getMaterialIndex());
 126         // The trick is to decrease the radius of the search sphere.
 127         _sphere.setRadius(length(closest - _sphere.getCenter()));
 128         _havePoint = true;
 129         _id = 0;
 130     }
 131
 132     void setSphere(const SGSphered& sphere)
 133     { _sphere = sphere; }
 134     const SGSphered& getSphere() const
 135     { return _sphere; }
 136
 137     const SGVec3d& getPoint() const
 138     { return _point; }
 139     const SGVec3d& getLinearVelocity() const
 140     { return _linearVelocity; }
 141     const SGVec3d& getAngularVelocity() const
 142     { return _angularVelocity; }
 143     const BVHMaterial* getMaterial() const
 144     { return _material; }
 145     BVHNode::Id getId() const
 146     { return _id; }
 147
 148     bool empty() const
 149     { return !_havePoint; }
 150
 151 private:
 152     SGSphered _sphere;
 153     double _time;
 154
 155     SGVec3d _point;
 156     SGVec3d _linearVelocity;
 157     SGVec3d _angularVelocity;
 158     const BVHMaterial* _material;
 159     BVHNode::Id _id;
 160
 161     bool _havePoint;
 162 };
 163
 164 }
 165
 166 #endif