src/rrtext2.cc

   1 #include "rrtext.h"
   2 #include "3D/Quaternion.h"
   3
   4 std::tuple<bool, unsigned int, unsigned int>
   5 RRTExt2::collide_steered_from(RRTNode &f)
   6 {
   7         {
   8                 dtSelectObject(&this->bc());
   9                 dtLoadIdentity();
  10                 dtTranslate(f.x(), f.y(), 0);
  11                 Quaternion q(Vector(0, 0, 1), f.h());
  12                 dtRotate(q[X], q[Y], q[Z], q[W]);
  13                 if (dtTest())
  14                         return std::make_tuple(true, 0, 0);
  15         }
  16         for (auto n: this->steered()) {
  17                 dtSelectObject(&this->bc());
  18                 dtLoadIdentity();
  19                 dtTranslate(n.x(), n.y(), 0);
  20                 Quaternion q(Vector(0, 0, 1), n.h());
  21                 dtRotate(q[X], q[Y], q[Z], q[W]);
  22                 if (dtTest())
  23                         return std::make_tuple(true, 0, 0);
  24         }
  25         return std::make_tuple(false, 0, 0);
  26 }
  27
  28 std::tuple<bool, unsigned int, unsigned int>
  29 RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t)
  30 {
  31         {
  32                 dtSelectObject(&this->bc());
  33                 dtLoadIdentity();
  34                 dtTranslate(f.x(), f.y(), 0);
  35                 Quaternion q(Vector(0, 0, 1), f.h());
  36                 dtRotate(q[X], q[Y], q[Z], q[W]);
  37                 if (dtTest())
  38                         return std::make_tuple(true, 0, 0);
  39         }
  40         {
  41                 dtSelectObject(&this->bc());
  42                 dtLoadIdentity();
  43                 dtTranslate(t.x(), t.y(), 0);
  44                 Quaternion q(Vector(0, 0, 1), t.h());
  45                 dtRotate(q[X], q[Y], q[Z], q[W]);
  46                 if (dtTest())
  47                         return std::make_tuple(true, 0, 0);
  48         }
  49         return std::make_tuple(false, 0, 0);
  50 }
  51
  52 void solid_collide(
  53         void * client_data,
  54         DtObjectRef obj1,
  55         DtObjectRef obj2,
  56         const DtCollData *coll_data
  57 )
  58 {
  59 }
  60
  61 void RRTExt2::init_solid()
  62 {
  63         this->sr_bc() = dtNewComplexShape();
  64         dtBegin(DT_POLYGON);
  65         dtVertex(this->bc().lfx(), this->bc().lfy(), 0);
  66         dtVertex(this->bc().lrx(), this->bc().lry(), 0);
  67         dtVertex(this->bc().rrx(), this->bc().rry(), 0);
  68         dtVertex(this->bc().rfx(), this->bc().rfy(), 0);
  69         dtEnd();
  70         dtEndComplexShape();
  71         dtCreateObject(&this->bc(), this->sr_bc());
  72
  73         this->sr_static_obstacles() = dtNewComplexShape();
  74         for (auto &o: this->obstacles()) {
  75                 dtBegin(DT_POLYGON);
  76                 for (auto c: o.poly()) {
  77                         dtVertex(std::get<0>(c), std::get<1>(c), 0);
  78                 }
  79                 dtEnd();
  80         }
  81         dtEndComplexShape();
  82         dtCreateObject(&this->static_obstacle(), this->sr_static_obstacles());
  83
  84         dtSetDefaultResponse(solid_collide, DT_SIMPLE_RESPONSE, stdout);
  85         dtDisableCaching(); // TODO enable
  86
  87         dtSelectObject(&this->bc());
  88         dtLoadIdentity();
  89         dtTranslate(this->nodes().front().x(), this->nodes().front().y(), 0);
  90         Quaternion q(Vector(0, 0, 1), this->nodes().front().h());
  91         dtRotate(q[X], q[Y], q[Z], q[W]);
  92 }
  93
  94 void RRTExt2::deinit_solid()
  95 {
  96         dtDeleteObject(&this->static_obstacle());
  97         dtDeleteObject(&this->bc());
  98         //dtDeleteShape(this->sr_bc());
  99         //dtDeleteShape(this->sr_static_obstacles());
 100 }