simgear/scene/bvh/bvhtest.cxx

   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 #include <iostream>
  19 #include <simgear/structure/SGSharedPtr.hxx>
  20
  21 #include "BVHNode.hxx"
  22 #include "BVHGroup.hxx"
  23 #include "BVHTransform.hxx"
  24
  25 #include "BVHStaticData.hxx"
  26
  27 #include "BVHStaticNode.hxx"
  28 #include "BVHStaticLeaf.hxx"
  29 #include "BVHStaticTriangle.hxx"
  30 #include "BVHStaticBinary.hxx"
  31 #include "BVHStaticGeometry.hxx"
  32
  33 #include "BVHBoundingBoxVisitor.hxx"
  34 #include "BVHSubTreeCollector.hxx"
  35 #include "BVHLineSegmentVisitor.hxx"
  36 #include "BVHNearestPointVisitor.hxx"
  37
  38 using namespace simgear;
  39
  40 BVHNode*
  41 buildSingleTriangle(const SGVec3f& v1, const SGVec3f& v2, const SGVec3f& v3)
  42 {
  43     BVHStaticData* staticData = new BVHStaticData;
  44     unsigned indices[3] = {
  45         staticData->addVertex(v1),
  46         staticData->addVertex(v2),
  47         staticData->addVertex(v3)
  48     };
  49     BVHStaticTriangle* staticTriangle = new BVHStaticTriangle(~0u, indices);
  50     return new BVHStaticGeometry(staticTriangle, staticData);
  51 }
  52
  53 bool
  54 testLineIntersections()
  55 {
  56     SGVec3f v1(-1, -1, 0);
  57     SGVec3f v2(1, -1, 0);
  58     SGVec3f v3(-1, 1, 0);
  59     SGSharedPtr<BVHNode> node = buildSingleTriangle(v1, v2, v3);
  60
  61     SGLineSegmentd lineSegment(SGVec3d(0, 0, -1), SGVec3d(0, 0, 1));
  62     {
  63         BVHLineSegmentVisitor lineSegmentVisitor(lineSegment);
  64         node->accept(lineSegmentVisitor);
  65         if (lineSegmentVisitor.empty())
  66             return false;
  67         if (!equivalent(lineSegmentVisitor.getPoint(), SGVec3d(0, 0, 0)))
  68             return false;
  69     }
  70
  71     SGVec3d position(1000, 1000, 1000);
  72     SGMatrixd matrix(position);
  73     SGSharedPtr<BVHTransform> transform1 = new BVHTransform;
  74     transform1->setToWorldTransform(matrix);
  75     transform1->addChild(node);
  76
  77     SGSharedPtr<BVHTransform> transform2 = new BVHTransform;
  78     transform2->setToLocalTransform(matrix);
  79     transform2->addChild(transform1);
  80
  81     {
  82         BVHLineSegmentVisitor lineSegmentVisitor(lineSegment);
  83         transform2->accept(lineSegmentVisitor);
  84         if (lineSegmentVisitor.empty())
  85             return false;
  86         if (!equivalent(lineSegmentVisitor.getPoint(), SGVec3d(0, 0, 0)))
  87             return false;
  88     }
  89
  90     SGSharedPtr<BVHMotionTransform> transform3 = new BVHMotionTransform;
  91     transform3->setLinearVelocity(SGVec3d(0, 0, 1));
  92     transform3->setAngularVelocity(SGVec3d(1, 0, 0));
  93     transform3->addChild(node);
  94
  95     {
  96         BVHLineSegmentVisitor lineSegmentVisitor(lineSegment, 0);
  97         transform3->accept(lineSegmentVisitor);
  98         if (lineSegmentVisitor.empty())
  99             return false;
 100         if (!equivalent(lineSegmentVisitor.getPoint(), SGVec3d(0, 0, 0)))
 101             return false;
 102         if (!equivalent(lineSegmentVisitor.getLinearVelocity(),
 103                         SGVec3d(0, 1, 1)))
 104             return false;
 105         if (!equivalent(lineSegmentVisitor.getAngularVelocity(),
 106                         SGVec3d(1, 0, 0)))
 107             return false;
 108     }
 109
 110     return true;
 111 }
 112
 113 bool
 114 testNearestPoint()
 115 {
 116     SGVec3f v1(-1, -1, 0);
 117     SGVec3f v2(1, -1, 0);
 118     SGVec3f v3(-1, 1, 0);
 119     SGSharedPtr<BVHNode> node = buildSingleTriangle(v1, v2, v3);
 120
 121     SGSphered sphere(SGVec3d(0, 0, -1), 2);
 122     {
 123       BVHNearestPointVisitor nearestPointVisitor(sphere, 0);
 124         node->accept(nearestPointVisitor);
 125         if (nearestPointVisitor.empty())
 126             return false;
 127         if (!equivalent(nearestPointVisitor.getPoint(), SGVec3d(0, 0, 0)))
 128             return false;
 129     }
 130
 131     return true;
 132 }
 133
 134 int
 135 main(int argc, char** argv)
 136 {
 137     if (!testLineIntersections())
 138         return EXIT_FAILURE;
 139     if (!testNearestPoint())
 140         return EXIT_FAILURE;
 141     return EXIT_SUCCESS;
 142 }