//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;
}
