大华摄像头三维定位

#define PI 3.1415926535898
bool dahua_dev::on_ptz_view_range(double *distance, double *nAngelH, double *nAngelV, double *nAzimuthH, double *nAzimuthV){
    DH_OUT_PTZ_VIEW_RANGE_STATUS ptzViewRangeStatus = {0};
    ptzViewRangeStatus.dwSize = sizeof(DH_OUT_PTZ_VIEW_RANGE_STATUS);
    int nRetLen = 0;
    bool bRet;
    bRet = CLIENT_QueryDevState(m_lLoginHandle, DH_DEVSTATE_PTZ_VIEW_RANGE, (char *) &ptzViewRangeStatus, sizeof(DH_OUT_PTZ_VIEW_RANGE_STATUS), &nRetLen, 3000);
    if(bRet){
        *nAngelH = ptzViewRangeStatus.nAngelH * 1.0 / 10 ;
        *nAngelV = ptzViewRangeStatus.nAngelV * 1.0 / 10 ;
        *nAngelH = *nAngelH / 2;
        *nAngelV = *nAngelV / 2;

        *nAzimuthH = ptzViewRangeStatus.nAzimuthH * 1.0 / 10;
        *nAzimuthV = ptzViewRangeStatus.nAzimuthV * 1.0 / 10;


        *distance = ptzViewRangeStatus.dbDistance;
        std::cout <<"on_ptz_view_range :::: " <<
                  "\n, 可视距离, 单位:米---> " << *distance <<
                  "\n,base_p 水平可视角度, 0~1800, 单位:十分之一度-->" << *nAngelH  <<
                  "\n,base_t 垂直可视角度, 0~1800, 单位:十分之一度-->" << *nAngelV  <<
                  "\n,f_w 水平方位角度, 0~3600, 单位:十分之一度-->" << *nAzimuthH  << "," <<
                  "\n,f_h 垂直方位角度, 0~3600, 单位:十分之一度-->" << *nAzimuthV  <<
                  "\n,水平倾斜角度, -900~900, 单位:十分之一度-->" << ptzViewRangeStatus.nInclinationH << "\n";
    }else{
        std::cout << "aaaaaaaaaaaaaaaaaaaaaaaaa";
        getlasterror();
    }
    return bRet;
}


bool dahua_dev::on_FocusONZoom(int xtop, int ytop, int xbottom, int ybotton,  int piWidth, int piHeight){
    double midW = piWidth * 1.0 / 2;
    double midH = piHeight * 1.0 / 2;

    if(midW <= 0 || midH <= 0 || xbottom - xtop <= 0 || ybotton - ytop <= 0  || xbottom > piWidth || ybotton > piHeight){
        std::cout << "xbottom - xtop = " << xbottom - xtop << ", ybotton - ytop = " << ybotton - ytop << "\n";
        std::cout << " xbottom > piWidth | ybotton > piHeight :: "<< xbottom << " > "<< piWidth << " , " << ybotton  << " > " << piHeight << ";" << "eeee\n";
        return false;
    }

    int  p = 0, t = 0, z = 0 ;
    dev_get_ptz(&p, &t, &z);




    auto center_x = (xtop + xbottom)  / 2;
    auto center_y = (ytop + ybotton)  / 2;
    double move_p = 0.0 , move_t = 0.0;
    double base_p = 0.0, base_t = 0.0;
    double f_w = 0.0, f_h = 0.0;
    double d1 = 0.0, d2 = 0.0;
    on_ptz_view_range(&d1,  &base_p, &base_t, &f_w, &f_h);



    if(center_y - midH > 0){
        move_t = -atan(- (midH - center_y) / midH * tan(base_t / 180 * PI)) * 180 / PI;
        move_t = move_t * 10;
        t = t  - (int)move_t; //  ../bin/hik -a 2 -b 742 -c 70 -d 832 -w 1920 -h 1080         557 29 1
    }else if(center_y - midH < 0){
        move_t = atan(- (center_y - midH) /midH * tan(base_t / 180 * PI)) * 180 / PI;
        move_t = move_t * 10;
        t = t  - (int)move_t; // ../bin/hik -a 33 -b 334 -c 158 -d 432 -w 1920 -h 1080    557 29 1
    }

    if(center_x - midW > 0){
        move_p = atan(  (center_x - midW) / midW * tan(base_p / 180 * PI)) * 180 / PI;
        move_p = move_p * 10;
        p = p  - (int)move_p;
    }else if(center_x - midW < 0){
        move_p =  (360 - atan((midW - center_x) / (midW) * tan(base_p / 180 * PI)) * 180 / PI);
        move_p = 3600 - move_p * 10;
        p = p  + (int)move_p;
    }

    auto picW = piWidth * 0.7 / (xbottom - xtop) ;
    auto picH = piHeight * 0.7 / (ybotton - ytop) ;
    auto k  = 1.0;
    if(picH < picW){
        k = picH;
    }else{
        k = picW;
    }
    if(k > 1){
       /* auto base = 1874.62 / 92.4099; // 128--> 可视距离,   1 --->可视距离92.4099
        z = (int)(k / (base / 127)) + z;
        if(z > 128){
            z = 128;
        }
      */
    }

    bool flag = false;
    flag = on_move_absolutfly(p, t , z);
    if(!flag){
        std::cout << "bool flag = on_move_absolutfly(p, t , z) error\t";
        getlasterror();
        return false;
    }

    bool can = can_movetoptz(&p, &t, &z);
    if(!can){
        std::cout << "bool can = can_movetoptz(&p, &t, &z) error\t";
        getlasterror();
        return false;
    }


    return true;
}