//private void Dubins::_solvePath() { qreal start[3] = {_posA.x(), _posA.y(), _angleA}; qreal end[3] = {_posB.x(), _posB.y(), _angleB}; _guts = QSharedPointer<DubinsPath>(new DubinsPath()); const int zeroIfGood = dubins_init(start, end, _minTurnRadius, _guts.data()); _isValid = false; if (zeroIfGood == 0) _isValid = true; }
double DubinsLength3D(double x0,double y0,double z0,double yaw0, double x1,double y1,double z1,double yaw1, double r) { std::cout<< "DubinsLength3D"<< std::endl; std::cout<< x0<<" "<<y0<<" "<<z0<<" "<<yaw0/M_PI*180.<< std::endl; std::cout<< x1<<" "<<y1<<" "<<z1<<" "<<yaw1/M_PI*180.<< std::endl; std::cout<<"rho: "<< r<< std::endl; DubinsPath path2D; double q0[]= { x0, y0, yaw0 }; double q1[]= { x1, y1, yaw1 }; dubins_init(q0, q1, r, &path2D); double length_2d= dubins_path_length(&path2D); return sqrt(length_2d*length_2d+(z0-z1)*(z0-z1) ); }
DubinsPath* setupDubins(AU_UAV_ROS::PlaneObject* pobj1, AU_UAV_ROS::PlaneObject* pobj2) { DubinsPath* dubinsPath = new DubinsPath; double q0[3]; double q1[3]; double* xyposition1 = findXYCoordinate(pobj1->getLongitude(),pobj1->getLatitude()); q0[0]=xyposition1[0]; q0[1]=xyposition1[1]; q0[2]=pobj1->getBearing()*DEGREES_TO_RADIANS; double* xyposition2 = findXYCoordinate(pobj2->getLongitude(),pobj2->getLatitude()); q1[0]=xyposition2[0]; q1[1]=xyposition2[1]; q1[2]=pobj2->getBearing()*DEGREES_TO_RADIANS; dubins_init(q0,q1,RHO,dubinsPath); return dubinsPath; }