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