/**
\brief Indicate I just received a RPL DIO from a neighbor.
This function should be called for each received a DIO is received so neighbor
routing information in the neighbor table can be updated.
The fields which are updated are:
- DAGrank
\param[in] msg The received message with msg->payload pointing to the DIO
header.
*/
void neighbors_indicateRxDIO(OpenQueueEntry_t* msg) {
uint8_t i;
uint8_t temp_8b;
// take ownership over the packet
msg->owner = COMPONENT_NEIGHBORS;
// update rank of that neighbor in table
neighbors_vars.dio = (icmpv6rpl_dio_ht*)(msg->payload);
// retrieve rank
temp_8b = *(msg->payload+2);
neighbors_vars.dio->rank = (temp_8b << 8) + *(msg->payload+3);
if (isNeighbor(&(msg->l2_nextORpreviousHop))==TRUE) {
for (i=0;i<MAXNUMNEIGHBORS;i++) {
if (isThisRowMatching(&(msg->l2_nextORpreviousHop),i)) {
if (
neighbors_vars.dio->rank > neighbors_vars.neighbors[i].DAGrank &&
neighbors_vars.dio->rank - neighbors_vars.neighbors[i].DAGrank >(DEFAULTLINKCOST*2*MINHOPRANKINCREASE)
) {
// the new DAGrank looks suspiciously high, only increment a bit
neighbors_vars.neighbors[i].DAGrank += (DEFAULTLINKCOST*2*MINHOPRANKINCREASE);
openserial_printError(COMPONENT_NEIGHBORS,ERR_LARGE_DAGRANK,
(errorparameter_t)neighbors_vars.dio->rank,
(errorparameter_t)neighbors_vars.neighbors[i].DAGrank);
} else {
neighbors_vars.neighbors[i].DAGrank = neighbors_vars.dio->rank;
}
break;
}
}
}
// update my routing information
neighbors_updateMyDAGrankAndNeighborPreference();
}
void neighbors_receiveDIO(OpenQueueEntry_t* msg,icmpv6rpl_dio_t* dio) {
uint8_t i;
// uint8_t temp_linkCost;
msg->owner = COMPONENT_NEIGHBORS;
if (isNeighbor(&(msg->l2_nextORpreviousHop))==TRUE) {
for (i=0;i<MAXNUMNEIGHBORS;i++) {
if (isThisRowMatching(&(msg->l2_nextORpreviousHop),i)) {
//neighbors_vars.neighbors[i].DAGrank = *((uint8_t*)(msg->payload));
//poipoi xv is the rang big endian??
neighbors_vars.neighbors[i].DAGrank = dio->rank;
// poipoi: single hop
/* if (neighbors_vars.neighbors[i].DAGrank==0x00) {
neighbors_vars.neighbors[i].parentPreference=MAXPREFERENCE;
if (neighbors_vars.neighbors[i].numTxACK==0) {
temp_linkCost=15; //TODO: evaluate using RSSI?
} else {
temp_linkCost=linkcost_calcETX(neighbors_vars.neighbors[i].numTx,neighbors_vars.neighbors[i].numTxACK);
}
if (idmanager_getIsDAGroot()==FALSE) {
neighbors_vars.myDAGrank=neighbors_vars.neighbors[i].DAGrank+temp_linkCost;
}
} else {
neighbors_vars.neighbors[i].parentPreference=0;
}*/
break;
}
}
}
// poipoi: single hop
neighbors_updateMyDAGrankAndNeighborPreference();
}
void registerNewNeighbor(open_addr_t* address,
int8_t rssi,
asn_t* asnTimestamp) {
uint8_t i,j;
bool iHaveAPreferedParent;
// filter errors
if (address->type!=ADDR_64B) {
openserial_printError(COMPONENT_NEIGHBORS,ERR_WRONG_ADDR_TYPE,
(errorparameter_t)address->type,
(errorparameter_t)2);
return;
}
// add this neighbor
if (isNeighbor(address)==FALSE) {
i=0;
while(i<MAXNUMNEIGHBORS) {
if (neighbors_vars.neighbors[i].used==FALSE) {
// add this neighbor
neighbors_vars.neighbors[i].used = TRUE;
neighbors_vars.neighbors[i].parentPreference = 0;
//neighbors_vars.neighbors[i].stableNeighbor = FALSE;
// poipoi: all new neighbors are consider stable
neighbors_vars.neighbors[i].stableNeighbor = TRUE;
neighbors_vars.neighbors[i].switchStabilityCounter = 0;
memcpy(&neighbors_vars.neighbors[i].addr_64b, address, sizeof(open_addr_t));
neighbors_vars.neighbors[i].DAGrank = 0xffff;
neighbors_vars.neighbors[i].rssi = rssi;
neighbors_vars.neighbors[i].numRx = 1;
neighbors_vars.neighbors[i].numTx = 0;
neighbors_vars.neighbors[i].numTxACK = 0;
memcpy(&neighbors_vars.neighbors[i].asn,asnTimestamp,sizeof(asn_t));
// do I already have a preferred parent ?
iHaveAPreferedParent = FALSE;
for (j=0;j<MAXNUMNEIGHBORS;j++) {
if (neighbors_vars.neighbors[j].parentPreference==MAXPREFERENCE) {
iHaveAPreferedParent = TRUE;
}
}
// if I have none, and I'm not DAGroot, the new neighbor is my preferred
if (iHaveAPreferedParent==FALSE && idmanager_getIsDAGroot()==FALSE) {
neighbors_vars.neighbors[i].parentPreference = MAXPREFERENCE;
}
break;
}
i++;
}
if (i==MAXNUMNEIGHBORS) {
openserial_printError(COMPONENT_NEIGHBORS,ERR_NEIGHBORS_FULL,
(errorparameter_t)MAXNUMNEIGHBORS,
(errorparameter_t)0);
return;
}
}
}
void floyd(DataForAI& data)
{
//initialize the graph
for(int i=1;i<=MAP_HEIGHT*MAP_WIDTH;i++)
for(int j=1;j<=MAP_HEIGHT*MAP_WIDTH;j++)
{
if(isNeighbor(i,j))
{//is neibors
if(data.map[iTop[i].row][iTop[i].col].type==STONE||data.map[iTop[j].row][iTop[j].col].type==STONE)
dist[i][j]=MAP_HEIGHT*MAP_WIDTH;
else if(data.map[iTop[i].row][iTop[i].col].type==BRICK||data.map[iTop[j].row][iTop[j].col].type==BRICK)
dist[i][j]=2;
else if(data.map[iTop[i].row][iTop[i].col].type==BREAKBRICK||data.map[iTop[j].row][iTop[j].col].type==BREAKBRICK)
dist[i][j]=1;
else
dist[i][j]=1;
}
else
dist[i][j]=MAP_HEIGHT*MAP_WIDTH;
}
//loop of floyd algorithm
for(int k=1;k<=MAP_HEIGHT*MAP_WIDTH;k++)
for(int i=1;i<=MAP_HEIGHT*MAP_WIDTH;i++)
for(int j=1;j<=MAP_HEIGHT*MAP_WIDTH;j++)
if(dist[i][k]+dist[k][j]<dist[i][j])
dist[i][j]=dist[i][k]+dist[k][j];
//printf("floyd complete!\n");
/*
for(int i=1;i<=MAP_HEIGHT*MAP_WIDTH;i++)
{
for(int j=1;j<=MAP_HEIGHT*MAP_WIDTH;j++)
printf("d(%d,%d)=%d ",i,j,dist[i][j]);
printf("\n");
}
*/
}
cocos2d::Vec2 SteeringBehaviors::separation()
{
Vec2 tmpForce;
auto tmpIterator = EntityMgr->getEntityMap()->begin();
for (; tmpIterator != EntityMgr->getEntityMap()->end(); tmpIterator++)
{
auto tmpCharacter = dynamic_cast<GameCharacter*>(tmpIterator->second);
if (tmpCharacter == m_pOwner || !isNeighbor(tmpCharacter))
{
continue;
}
auto tmpToOwner = m_pOwner->getMovingEntity().getPosition() - tmpCharacter->getMovingEntity().getPosition();
// 这个驱动力与该角色到owner的距离成反比（注意这里的距离是减去了两个半径后）
auto tmpLen = tmpToOwner.getLength() - (m_pOwner->getMovingEntity().getRadius() + tmpCharacter->getMovingEntity().getRadius());
if (tmpLen <= 0)
{
tmpLen = 1;
}
tmpForce += m_separationMagnify * tmpToOwner.getNormalized() / tmpLen;
}
return tmpForce;
}
/*trySwapPieces
* compute the score the puzzle WOULD have, if swap occured
* p:puzzle
* i, j: pieces to be swap
* returns: new total score with the swap
*
*/
int trySwapPieces(puzzle p,int **cur_sol,int score, int i, int j, int *wrongs, int *wsize_ptr){
//newscores=oldscores-computeLocalScore(....,cur_sol, i)-computeLocalScore(.....,cur_sol, j)
//.... +computeLocalScore(...., swapped_sol, i)+computeLocalScore(....,swapped_sol, j);
int rot, tmp, old_i, old_j, old_rot_i, old_rot_j, k, maxi=0, maxj=0, max_rot_i, max_rot_j, found, w, skip=0;
int scoreold=score;
//store actual pieces and rotations:
old_i=cur_sol[i][0];
old_j=cur_sol[j][0];
old_rot_i=cur_sol[i][1];
old_rot_j=cur_sol[j][1];
score-=computeLocalScore(p, cur_sol, cur_sol[i][0], i, cur_sol[i][1]);
score-=computeLocalScore(p, cur_sol, cur_sol[j][0], j, cur_sol[j][1]);
if(isNeighbor(p, i, j)){
/* try rotating the two pieces in place */
score+=checkEdge(p, cur_sol, i, j); // score for common edge was removed twice, so re-add once
for(cur_sol[i][1]=0;cur_sol[i][1]<4;cur_sol[i][1]++){ //rotate i
for(cur_sol[j][1]=0;cur_sol[j][1]<4;cur_sol[j][1]++){ //rotate j
k=computeLocalScore(p, cur_sol, old_i, i, cur_sol[i][1]); //score from i
tmp=computeLocalScore(p, cur_sol, old_j, j, cur_sol[j][1]); //score from j
if(tmp+k>maxi+maxj){
maxi=k;
maxj=tmp;
max_rot_i=cur_sol[i][1];
max_rot_j=cur_sol[j][1];
}
}
}
score+=maxi+maxj; // this adds one point for each "good" edge, so the common edge has double score
//update solution
cur_sol[i][1]=max_rot_i;
cur_sol[j][1]=max_rot_j;
score-=checkEdge(p, cur_sol, i, j); //score for common edge has been added twice, then subtract once
if (score>scoreold){
skip=1; //-> don't try swapping; maybe next time...
} else {
//restore
cur_sol[i][1]=old_rot_i;
cur_sol[j][1]=old_rot_j;
score-=maxi+maxj;
}
if(!skip){
/* swap the pieces (if leads to better score) */
cur_sol[i][0]=old_j; //move j-->i
cur_sol[j][0]=old_i; //move i-->j
for(cur_sol[i][1]=0;cur_sol[i][1]<4;cur_sol[i][1]++){ //rotate i
for(cur_sol[j][1]=0;cur_sol[j][1]<4;cur_sol[j][1]++){ //rotate j
k=computeLocalScore(p, cur_sol, old_j, i, cur_sol[i][1]); //score from i
tmp=computeLocalScore(p, cur_sol, old_i, j, cur_sol[j][1]); //score from j
if(tmp+k>maxi+maxj){
maxi=k;
maxj=tmp;
max_rot_i=cur_sol[i][1];
max_rot_j=cur_sol[j][1];
}
}
}
score+=maxi+maxj; // this adds one point for each "good" edge, so the common edge has double score
score-=checkEdge(p, cur_sol, i, j); //score for common edge has been added twice, then subtract once
if (score>scoreold){
//update solution
cur_sol[i][1]=max_rot_i;
cur_sol[j][1]=max_rot_j;
} else {
//restore old values:
cur_sol[i][0]=old_i;
cur_sol[i][1]=old_rot_i;
cur_sol[j][0]=old_j;
cur_sol[j][1]=old_rot_j;
score=scoreold;
}
}
} else {
for(rot=0;rot<4;rot++){
tmp=computeLocalScore(p, cur_sol, old_i, j, rot);
if(tmp>maxi){
maxi=tmp;
max_rot_i=rot;
}
}
score+=maxi;
maxj=0;
for(rot=0;rot<4;rot++){
tmp=computeLocalScore(p, cur_sol, old_j, i, rot);
if(tmp>maxj){
maxj=tmp;
max_rot_j=rot;
}
}
score+=maxj;
if(score>scoreold){
cur_sol[j][0]=old_i;
cur_sol[j][1]=max_rot_i;
cur_sol[i][0]=old_j;
cur_sol[i][1]=max_rot_j;
}else{
return(scoreold);
}
}
/* update wrongs vector */
#ifdef WRONGS
if(maxi==MAX_LOCAL_SCORE){
removeWrong(p, cur_sol, wrongs, wsize_ptr, i);
}
// update neighbors of i
updateWrongs(p, cur_sol, wrongs, wsize_ptr, i);
if(maxj==MAX_LOCAL_SCORE){
removeWrong(p, cur_sol, wrongs, wsize_ptr, i);
}
// update neighbors of j
updateWrongs(p, cur_sol, wrongs, wsize_ptr, i);
#endif
return(score);
}
void MeshConversion<SensorT>::fromPointCloud(
const typename pcl::PointCloud<PointT>::ConstPtr& pc, MeshT& mesh)
{
typedef typename MeshT::VertexHandle VertexHandle;
int rows = pc->height - 1; // last row
int cols = pc->width - 1; // last column
int row_offset;
// [h]orizontal, [v]ertical, [l]eft, [r]ight edge check
std::vector<std::vector<bool> > h(rows+1, std::vector<bool>(cols,true));
std::vector<std::vector<bool> > v(rows, std::vector<bool>(cols+1,true));
std::vector<std::vector<bool> > l(rows, std::vector<bool>(cols,true));
std::vector<std::vector<bool> > r(rows, std::vector<bool>(cols,true));
std::vector<std::vector<VertexHandle> >
vh(rows+1, std::vector<VertexHandle>(cols+1)); // vertex handles
/*
* +--+--+ p00 h00 p01 h01 p02
* | | | v00 lr00 v01 lr01 v02
* +--+--+ p10 h10 p11 h11 p12
* | | | v10 lr10 v11 lr11 v12
* +--+--+ p20 h20 p21 h21 p22
*/
// corners
h.front().front() = v.front().front() = r.front().front() = false;
h.front().back() = v.front().back() = l.front().back() = false;
h.back().front() = v.back().front() = l.back().front() = false;
h.back().back() = v.back().back() = r.back().back() = false;
// first and last row
for(int x = 1; x<cols; ++x)
{
h.front()[x-1] = false;
h.front()[x ] = false;
v.front()[x ] = false;
l.front()[x-1] = false;
r.front()[x ] = false;
h.back ()[x-1] = false;
h.back ()[x ] = false;
v.back ()[x ] = false;
r.back ()[x-1] = false;
l.back ()[x ] = false;
}
for(int y = 1; y<rows; ++y)
{
// left column and right column
h[y ].front() = false;
v[y-1].front() = false;
v[y ].front() = false;
l[y-1].front() = false;
r[y ].front() = false;
h[y ].back() = false;
v[y-1].back() = false;
v[y ].back() = false;
r[y-1].back() = false;
l[y ].back() = false;
row_offset = y*(cols+1);
// iterate remaining
for(int x=1; x<cols; ++x)
{
const PointT* p = &(*pc)[row_offset+x];
if( p->z != p->z )
{
v[y-1][x ] = false;
v[y ][x ] = false;
h[y ][x-1] = false;
h[y ][x ] = false;
l[y-1][x ] = false;
l[y ][x-1] = false;
r[y-1][x-1] = false;
r[y ][x ] = false;
}
else
{
vh[y][x] = mesh.addVertex(y*pc->width+x, *p);
}
}
}
// iterate h and v to check if edge is valid
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pii = pc->points.begin();
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pij = pii + 1; // right
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pji = pii + 1 + cols; // below
typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> >
::const_iterator pjj = pji + 1; // below right
for(int y=0; y<rows; ++y)
{
for(int x=0; x<cols; ++x)
{
// check horizontal and vertical
if (h[y][x])
h[y][x] = isNeighbor(pii->getVector3fMap(), pij->getVector3fMap());
if (v[y][x])
v[y][x] = isNeighbor(pii->getVector3fMap(), pji->getVector3fMap());
// check diagonal
unsigned char status = (l[y][x] << 1) | r[y][x];
switch(status)
{
case 0b00:
break;
case 0b01:
r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap());
break;
case 0b10:
l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap());
break;
case 0b11:
if( (pij->z - pji->z) > (pii->z - pjj->z) )
{
r[y][x] = false;
l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap());
}
else
{
l[y][x] = false;
r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap());
}
break;
}
++pii; ++pij; ++pji; ++pjj;
}
// skip the last column
// note that in the very last iteration, pjj points beyond end()
++pii; ++pij; ++pji; ++pjj;
}
for(int y=0; y<rows; ++y)
{
for(int x=0; x<cols; ++x)
{
/* ii-ji-ij | ij-ji-jj | ii-jj-ij | ii-ji-jj
* +-+ | + | +-+ | +
* |/ | /| | \| | |\
* + | +-+ | + | +-+ */
if(l[y][x])
{
if (h[y ][x] && v[y][x ])
mesh.addFace(vh[y][x ], vh[y+1][x], vh[y ][x+1]);
if (h[y+1][x] && v[y][x+1])
mesh.addFace(vh[y][x+1], vh[y+1][x], vh[y+1][x+1]);
}
else if (r[y][x])
{
if (h[y][x] && v[y][x+1])
mesh.addFace(vh[y][x], vh[y+1][x+1], vh[y][x+1]);
if (v[y][x] && h[y+1][x])
mesh.addFace(vh[y][x], vh[y+1][x], vh[y+1][x+1]);
}
}
}
}
std::shared_ptr<MatchHash> MatchArray::findMatch(Coordinates const& chunkPos)
{
std::shared_ptr<MatchHash> strongPtr;
{
ScopedMutexLock cs(mutex);
std::weak_ptr<MatchHash>& weakPtr = matches[chunkPos];
strongPtr = weakPtr.lock();
if (strongPtr) {
Event::ErrorChecker ec;
while (strongPtr->busy) {
strongPtr->waiting = true;
event.wait(mutex, ec);
}
return strongPtr;
} else {
weakPtr = strongPtr = std::shared_ptr<MatchHash>(new MatchHash());
strongPtr->busy = true;
}
}
std::shared_ptr<ConstArrayIterator> patternIterator = pattern->getConstIterator(patternIteratorAttr);
std::shared_ptr<ConstArrayIterator> catalogIterator = catalog->getConstIterator(catalogIteratorAttr);
if (patternIterator->setPosition(chunkPos) && catalogIterator->setPosition(chunkPos))
{
ConstChunk const& catalogChunk = catalogIterator->getChunk();
ConstChunk const& patternChunk = patternIterator->getChunk();
MatchHash catalogHash(catalogChunk.count());
for (std::shared_ptr<ConstChunkIterator> ci = catalogChunk.getConstIterator(ConstChunkIterator::IGNORE_EMPTY_CELLS);
!ci->end();
++(*ci))
{
catalogHash.addCatalogEntry(ci->getPosition(), 0, 0, error);
}
MatchHash* patternHash = strongPtr.get();
patternHash->table.resize(patternChunk.count() + HASH_TABLE_RESERVE);
int64_t item_no = 0;
for (std::shared_ptr<ConstChunkIterator> pi = patternChunk.getConstIterator(ConstChunkIterator::IGNORE_EMPTY_CELLS);
!pi->end();
++(*pi), item_no++)
{
Coordinates const& patternPos = pi->getPosition();
int64_t hash = getCatalogHash(patternPos, error);
for (MatchHash::Elem* elem = catalogHash.collisionChain(hash); elem != NULL; elem = elem->collisionChain) {
if (elem->hash == hash && isNeighbor(patternPos, elem->coords, error)) {
MatchHash::Elem*& chain = patternHash->collisionChain(item_no);
chain = new MatchHash::Elem(elem->coords, item_no, chain);
}
}
}
strongPtr->initialized = true;
}
{
ScopedMutexLock cs(mutex);
strongPtr->busy = false;
if (strongPtr->waiting) {
strongPtr->waiting = false;
event.signal();
}
}
return strongPtr;
}
