local void ball_search(smxptr sm, real r2ball, real *ri) { kdnode *ntab = sm->kd->ntab; bodyptr *bptr = sm->kd->bptr; pqnode *pq = sm->pqhead; int cell, cp, ct, pj; real dist2; cell = KDROOT; /* start at root of tree */ while (cell < sm->kd->nsplit) { /* descend to local bucket */ if (ri[ntab[cell].dim] < ntab[cell].split) cell = Lower(cell); else cell = Upper(cell); } for (pj = ntab[cell].first; pj <= ntab[cell].last; ++pj) if (! InQue(bptr[pj])) { /* in bucket, but not que? */ DISTSQV(dist2, ri, Pos(bptr[pj])); /* compute dist^2 to center */ if (dist2 < r2ball) { /* within current ball? */ ClrFlag(bptr[pq->pind], INQUE); /* drop furthest from que */ SetFlag(bptr[pj], INQUE); /* and add this one to que */ pq->pqkey = dist2; /* store its distance */ pq->pind = pj; /* and its index */ PQReplace(pq); /* move to rightful place */ r2ball = pq->pqkey; /* adopt new search radius */ } } while (cell != KDROOT) { /* scan back toward root */ cp = Sibling(cell); ct = cp; SetNext(ct); do { Intersect(ntab[cp], r2ball, ri, GetNextCell); /* got intersection to test */ if (cp < sm->kd->nsplit) { /* not yet down to bucket? */ cp = Lower(cp); continue; } else /* scan bucket for winners */ for (pj = ntab[cp].first; pj <= ntab[cp].last; ++pj) if (! InQue(bptr[pj])) { /* not already in the que? */ DISTSQV(dist2, ri, Pos(bptr[pj])); if (dist2 < r2ball) { /* but within current ball? */ ClrFlag(bptr[pq->pind], INQUE); SetFlag(bptr[pj], INQUE); pq->pqkey = dist2; pq->pind = pj; PQReplace(pq); r2ball = pq->pqkey; } } GetNextCell: SetNext(cp); } while (cp != ct); cell = Parent(cell); /* climb down towards root */ } sm->pqhead = pq; }
/***************************************************************************** * 函 数 名 : HSUART_Recv * * 功能描述 : HS UART接收数据接口函数,读取有效数据时需要 * 4字节对齐,否则会多读到后面的有效数据并丢弃, * 下次读取会失败。 * * 输入参数 : UINT8 * pu8DstAddr 接收到的数据存放的的首地址 * UINT32 u32TransLength 接收的数据的长度 * 输出参数 : 无 * 返 回 值 : OK 成功 * ERROR 失败 * * 修改记录 :2010年12月16日 鲁婷 创建 *****************************************************************************/ HSUART_STATUS hsUartRecv(RECV_STR *pstHsUartRecvData) { UINT32 ulCharNum; UINT32 ulData; UINT8* ucTemp; UINT32 i; UINT32 ulInt = 0; ulInt = INREG32(HSUART_BASE_ADDR + UART_IIR); ulInt &= 0xF; if(ulInt == 0xC) { ulCharNum = INREG32(HSUART_BASE_ADDR + UART_RFL); while(ulCharNum) { if(ulCharNum >= 4) { ulData = INREG32(HSUART_BASE_ADDR + UART_RBR); ucTemp = (UINT8*)&ulData; for(i=0;i<4;i++) { if(InQue(pstHsUartRecvData, ucTemp[i]) != OK) { print_info("\r\nbuffer full\r\n"); return HSUART_STATUS_BUFFER_FULL_ERR; } } ulCharNum = ulCharNum - 4; } else { ulData = INREG32(HSUART_BASE_ADDR + UART_RBR); ucTemp = (UINT8*)&ulData; for(i=0;i<ulCharNum;i++) { if(InQue(pstHsUartRecvData, ucTemp[i]) != OK) { print_info("\r\nbuffer full\r\n"); return HSUART_STATUS_BUFFER_FULL_ERR; } } return HSUART_STATUS_OK; } //ulCharNum = INREG32(HSUART_BASE_ADDR + UART_RFL); } } else if(ulInt == 0x4) { ulCharNum = INREG32(HSUART_BASE_ADDR + UART_USR); while(ulCharNum & 0x8) { ulData = INREG32(HSUART_BASE_ADDR + UART_RBR); ucTemp = (UINT8*)&ulData; for(i=0;i<4;i++) { if(InQue(pstHsUartRecvData, ucTemp[i]) != OK) { print_info("\r\nbuffer full\r\n"); return HSUART_STATUS_BUFFER_FULL_ERR; } } ulCharNum = INREG32(HSUART_BASE_ADDR + UART_USR); } } return HSUART_STATUS_OK; }