C++ U2函数代码示例

/* decode ubx-nav-pvt: navigation pvt -----------------------------------*/
int decode_navpvt(tUbxRawData *raw)
{
    UbxMsg01_07_PVT pvt;
    unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;

    memset(&pvt, 0, sizeof(pvt));

    pvt.iTOW  = U4(p);
    pvt.year  = U2(p+4);
    pvt.month = U1(p+6);
    pvt.day   = U1(p+7);
    pvt.hour  = U1(p+8);
    pvt.min   = U1(p+9);
    pvt.sec   = U1(p+10);

    pvt.bitfield = U1(p+11);
    pvt.timeUnc  = U4(p+12);
    pvt.nano     = I4(p+16);

    pvt.fixType  = U1(p+20);
    pvt.flags    = U1(p+21);
    pvt.flags2   = U1(p+22);
    pvt.numSV    = U1(p+23);

    pvt.Lon      = I4(p+24);
    pvt.Lat      = I4(p+28);
    pvt.Alt      = I4(p+32);
    pvt.AltMsl   = I4(p+36);

    pvt.hErr     = U4(p+40);
    pvt.vErr     = U4(p+44);

    pvt.Vn       = I4(p+48);
    pvt.Ve       = I4(p+52);
    pvt.Vd       = I4(p+56);

    pvt.groundSpeed = I4(p+60);
    pvt.heading     = I4(p+64);
    pvt.spdErr      = U4(p+68);
    pvt.headingErr  = U4(p+72);

    pvt.PDOP        = U2(p+76);
    
    memcpy(pvt.reserved1, p+78, 6*sizeof(unsigned char));

    pvt.headVel     = I4(p+84);

    memcpy(pvt.reserved2, p+88, 4*sizeof(unsigned char));

    /* fill up interface for real time processing */
    pb_Set_ExternalUbxGNSS_PVT(&pvt);

    /*----- validate PPS -----*/
    pb_Validate_Ubx_PPS(&pvt);
	
    return 0;
}

Code::Code(AttributeInfo *code) {
    unsigned char *data = code->data;
    int index = 0;
    U2(maxStack,data,index);
    U2(maxLocals,data,index);
    U4(codeLength,data,index);
    this->code = new unsigned char[codeLength];
    for (int i = 0; i < codeLength; i++) {
        this->code[i] = data[index+i];
    }
    index += codeLength;
    U2(exceptionCount,data,index);
    exceptions = new ExceptionEntry*[exceptionCount];
    for (int i = 0; i < exceptionCount; i++) {
        exceptions[i] = new ExceptionEntry;
        U2(exceptions[i]->startPC,data,index);
        U2(exceptions[i]->endPC,data,index);
        U2(exceptions[i]->handlerPC,data,index);
        U2(exceptions[i]->catchType,data,index);
    }
    U2(attributeCount,data,index);
    attributes = new AttributeInfo*[attributeCount];
    for (int i = 0; i < attributeCount; i++) 
        attributes[i] = new AttributeInfo(data, index);
}

/* input ublox raw message from file -------------------------------------------
* fetch next ublox raw data and input a message from file
* args   : raw_t  *raw   IO     receiver raw data control struct
*          FILE   *fp    I      file pointer
* return : status(-2: end of file, -1...9: same as above)
*-----------------------------------------------------------------------------*/
extern int input_ubxf(raw_t *raw, FILE *fp)
{
    int i,data;
    
    trace(4,"input_ubxf:\n");
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        for (i=0;;i++) {
            if ((data=fgetc(fp))==EOF) return -2;
            if (sync_ubx(raw->buff,(unsigned char)data)) break;
            if (i>=4096) return 0;
        }
    }
    if (fread(raw->buff+2,1,4,fp)<4) return -2;
    raw->nbyte=6;
    
    if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
        trace(2,"ubx length error: len=%d\n",raw->len);
        raw->nbyte=0;
        return -1;
    }
    if (fread(raw->buff+6,1,raw->len-6,fp)<(size_t)(raw->len-6)) return -2;
    raw->nbyte=0;
    
    /* decode ubx raw message */
    return decode_ubx(raw);
}

/* input ublox raw message from stream -----------------------------------------
* fetch next ublox raw data and input a mesasge from stream
* args   : raw_t *raw   IO     receiver raw data control struct
*            raw->opt : u-blox raw options
*                "-invcp" : inversed polarity of carrier-phase
*                "-EPHALL"  : output all ephemerides
*          unsigned char data I stream data (1 byte)
* return : status (-1: error message, 0: no message, 1: input observation data,
*                  2: input ephemeris, 3: input sbas message,
*                  9: input ion/utc parameter)
*-----------------------------------------------------------------------------*/
extern int input_ubx(raw_t *raw, unsigned char data)
{
    trace(5,"input_ubx: data=%02x\n",data);
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        if (!sync_ubx(raw->buff,data)) return 0;
        raw->nbyte=2;
        return 0;
    }
    raw->buff[raw->nbyte++]=data;
    
    if (raw->nbyte==6) {
        if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
            trace(2,"ubx length error: len=%d\n",raw->len);
            raw->nbyte=0;
            return -1;
        }
    }
    if (raw->nbyte<6||raw->nbyte<raw->len) return 0;
    raw->nbyte=0;
    
    /* decode ublox raw message */
    return decode_ubx(raw);
}

int testmaxmin() {
	vector<double> x;
	x.push_back(1);
	x.push_back(2);
	x.push_back(3);
	x.push_back(1.1);
	x.push_back(1);
	x.push_back(9);
	x.push_back(1.01);
	x.push_back(11);
	x.push_back(0.001);
	vector<double> U(x.size()),L(x.size());
	vector<double> U2(x.size()),L2(x.size());
	for(uint constraint = 0; constraint<x.size(); ++constraint) {
		
		computeEnvelope(x,constraint,U,L);
		Envelope env;
		env.compute(x,constraint,U2,L2);
		if((U!=U2) or (L!=L2)) { cout<< "bug! "<<constraint<<endl;
		cout <<" x U L U2 L2"<<endl;
		for(uint k = 0; k<x.size();++k)
		  cout<<"k="<<k<<" "<<x[k]<<" "<< U[k]<<" "<< L[k]<<" " <<U2[k]<<" "<< L2[k]<<endl;
		}
		return -1;
	}
	return 0;
	
}

/** ptr: &uinfo[] */
void U1(struct user_info * p)
{
	xlog(LOG_U, "U1(). %s arrives at floor %d, destination is %d.\n",
	     p->name, p->IN, p->OUT);

	U2(p);
}

static HRESULT WINAPI enummodescallback(LPDDSURFACEDESC lpddsd, LPVOID lpContext)
{
    trace("Width = %i, Height = %i, Refresh Rate = %i, Pitch = %i, flags =%02X\r\n",
        lpddsd->dwWidth, lpddsd->dwHeight,
          U2(*lpddsd).dwRefreshRate, U1(*lpddsd).lPitch, lpddsd->dwFlags);

    /* Check that the pitch is valid if applicable */
    if(lpddsd->dwFlags & DDSD_PITCH)
    {
        ok(U1(*lpddsd).lPitch != 0, "EnumDisplayModes callback with bad pitch\n");
    }

    /* Check that frequency is valid if applicable
     *
     * This fails on some Windows drivers or Windows versions, so it isn't important
     * apparently
    if(lpddsd->dwFlags & DDSD_REFRESHRATE)
    {
        ok(U2(*lpddsd).dwRefreshRate != 0, "EnumDisplayModes callback with bad refresh rate\n");
    }
     */

    adddisplaymode(lpddsd);

    return DDENUMRET_OK;
}

/* input skytraq raw message from stream ---------------------------------------
* fetch next skytraq raw data and input a mesasge from stream
* args   : raw_t *raw   IO     receiver raw data control struct
*            raw->opt : skytraq raw options
*                "-invcp" : inversed polarity of carrier-phase
*          unsigned char data I stream data (1 byte)
* return : status (-1: error message, 0: no message, 1: input observation data,
*                  2: input ephemeris, 3: input sbas message,
*                  9: input ion/utc parameter)
*-----------------------------------------------------------------------------*/
extern int input_stq(raw_t *raw, unsigned char data)
{
    trace(5,"input_stq: data=%02x\n",data);
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        if (!sync_stq(raw->buff,data)) return 0;
        raw->nbyte=2;
        return 0;
    }
    raw->buff[raw->nbyte++]=data;
    
    if (raw->nbyte==4) {
        if ((raw->len=U2(raw->buff+2)+7)>MAXRAWLEN) {
            trace(2,"stq message length error: len=%d\n",raw->len);
            raw->nbyte=0;
            return -1;
        }
    }
    if (raw->nbyte<4||raw->nbyte<raw->len) return 0;
    raw->nbyte=0;
    
    /* decode skytraq raw message */
    return decode_stq(raw);
}

/* input skytraq raw message from file -----------------------------------------
* fetch next skytraq raw data and input a message from file
* args   : raw_t  *raw   IO     receiver raw data control struct
*          FILE   *fp    I      file pointer
* return : status(-2: end of file, -1...9: same as above)
*-----------------------------------------------------------------------------*/
extern int input_stqf(raw_t *raw, FILE *fp)
{
    int i,data;
    
    trace(4,"input_stqf:\n");
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        for (i=0;;i++) {
            if ((data=fgetc(fp))==EOF) return -2;
            if (sync_stq(raw->buff,(unsigned char)data)) break;
            if (i>=4096) return 0;
        }
    }
    if (fread(raw->buff+2,1,2,fp)<2) return -2;
    raw->nbyte=4;
    
    if ((raw->len=U2(raw->buff+2)+7)>MAXRAWLEN) {
        trace(2,"stq message length error: len=%d\n",raw->len);
        raw->nbyte=0;
        return -1;
    }
    if (fread(raw->buff+4,1,raw->len-4,fp)<(size_t)(raw->len-4)) return -2;
    raw->nbyte=0;
    
    /* decode skytraq raw message */
    return decode_stq(raw);
}

void rubikStep(char *step)
{
	u8 m=0;
	for(m=0;step[m]!=0;m++)
	{
		switch(step[m])
		{
			case 7:allright90();break;
			case 11:F1();break;
			case 12:F2();break;
			case 13:F3();break;
			case 21:B1();break;
			case 22:B2();break;
			case 23:B3();break;
			case 31:R1();break;
			case 32:R2();break;
			case 33:R3();break;
			case 41:L1();break;
			case 42:L2();break;
			case 43:L3();break;
			case 51:U1();break;
			case 52:U2();break;
			case 53:U3();break;
			case 61:D1();break;
			case 62:D2();break;
			case 63:D3();break;
			default:break;
		}
	}
}

/* decode solution -----------------------------------------------------------*/
static int decode_gw10sol(raw_t *raw)
{
    gtime_t time;
    double ep[6]={0},sec;
    unsigned char *p=raw->buff+6;
    
    trace(4,"decode_gw10sol : len=%d\n",raw->len);
    
    if (U2(p+42)&0xC00) { /* time valid? */
        trace(2,"gw10 sol time/day invalid\n");
        return 0;
    }
    sec=U4(p+27)/16384.0;
    sec=floor(sec*1000.0+0.5)/1000.0;
    ep[2]=bcd2num(p[31]);
    ep[1]=bcd2num(p[32]);
    ep[0]=bcd2num(p[33])*100+bcd2num(p[34]);
    time=utc2gpst(timeadd(epoch2time(ep),sec));
    
    /* set time if no time available */
    if (raw->time.time==0) {
        raw->time=time;
    }
    return 0;
}

void 
MultivariateModel
::ComputeOrthonormalBasis() 
{
  /// It computes a basis of vector orthogonal to the geodesic gamma_derivative at t0, with respect to
  /// the scalar product defined on the riemannian manifold
  /// Further information about the mathematical operation in the documentation
  
  ScalarType V0 = - m_G / (m_G + 1) * exp(m_RandomVariables.GetRandomVariable("Ksi")->GetParameter("Mean"));
  VectorType U(m_ManifoldDimension);
  ScalarType * u = U.memptr();
  ScalarType * d = m_Deltas.memptr();
  
  for(size_t i = 0; i < m_ManifoldDimension; ++i)
    u[i] = V0 * (1.0/m_G + exp(d[i]));
  
  /// Compute the initial pivot vector U
  double Norm = U.magnitude();
  U(0) += copysign(1, -U(0)) * Norm;
      
  // Check the vectorise_row_wise function of the ArmadilloMatrixWrapper
  MatrixType U2(U);
  double NormU2 = U.squared_magnitude();
  MatrixType FinalMatrix2 = (-2.0/NormU2) * U2*U2.transpose();
  for(size_t i = 0; i < m_ManifoldDimension; ++i)
    FinalMatrix2(i, i ) += 1;
  
  m_OrthogonalBasis = FinalMatrix2;
}

/*--- input_ubx is supposed to be called from tread ---*/
int input_ubx(tUbxRawData *raw, unsigned char data)
{
    /* synchronize frame */
    if (raw->nbyte == 0) 
    {
        if (!sync_ubx(raw->buff, data)) 
           return 0;
        
        raw->nbyte=2;
        
        return 0;        /* wait for new bytes join in */
    }

    raw->buff[raw->nbyte++]=data;
    
    if (raw->nbyte == UBX_MSGSTART_SHIFT)     
    {
       raw->length = U2(raw->buff+4) + 8; 
       if (raw->length > MAXRAWLEN) 
       {
          raw->nbyte = 0;
          return -1;
       }
    }

    if((raw->nbyte < UBX_MSGSTART_SHIFT) || (raw->nbyte < raw->length))
       return 0;             /* return until length is satisfied */
    
    raw->nbyte = 0;
    raw->frameReady = TRUE;
		
		memcpy(&readyRawUbx, raw, sizeof(tUbxRawData));
    return 0;
}

bool TTetrahedron::IsInner(const TPoint& P) const {
    TVector N, U1(X1, X2), U2(X1, X3), U3(X1, X4), U4(X2, X3), U5(X2, X4), U6(X2, X1);
    TVector Up1(X1, P), Up2(X2, P);

    N = Cross(U1, U2); //X4 is on opposite side of plain X1,X2,X3 than P
    if (Dot(N, U3) * Dot(N, Up1) < 0) {
        return false;
    }

    N = Cross(U1, U3); //X3 x P
    if (Dot(N, U2) * Dot(N, Up1) < 0) {
        return false;
    }

    N = Cross(U2, U3); //X2 x P
    if (Dot(N, U1) * Dot(N, Up1) < 0) {
        return false;
    }

    N = Cross(U4, U5); //X1 x P
    if (Dot(N, U6) * Dot(N, Up2) < 0) {
        return false;
    }

    return true;
}

static void test_title(void)
{
    HPROPSHEETPAGE hpsp[1];
    PROPSHEETPAGEA psp;
    PROPSHEETHEADERA psh;
    HWND hdlg;

    memset(&psp, 0, sizeof(psp));
    psp.dwSize = sizeof(psp);
    psp.dwFlags = 0;
    psp.hInstance = GetModuleHandleW(NULL);
    U(psp).pszTemplate = "prop_page1";
    U2(psp).pszIcon = NULL;
    psp.pfnDlgProc = page_dlg_proc;
    psp.lParam = 0;

    hpsp[0] = CreatePropertySheetPageA(&psp);

    memset(&psh, 0, sizeof(psh));
    psh.dwSize = sizeof(psh);
    psh.dwFlags = PSH_MODELESS | PSH_USECALLBACK;
    psh.pszCaption = "test caption";
    psh.nPages = 1;
    psh.hwndParent = GetDesktopWindow();
    U3(psh).phpage = hpsp;
    psh.pfnCallback = sheet_callback;

    hdlg = (HWND)PropertySheetA(&psh);
    DestroyWindow(hdlg);
}