bool Map::CheckMazeTileConn(int index_1,int openDir,int index_2)
{
//true means cannot pass
if(openDir==0&&index_1-_width==index_2)
{
CheckBorder(index_2);
if(!_border[2])
return true;
}
if(openDir==1&&index_1+1==index_2)
{
CheckBorder(index_2);
if(!_border[3])
return true;
}
if(openDir==2&&index_1+_width==index_2)
{
CheckBorder(index_2);
if(!_border[0])
return true;
}
if(openDir==3&&index_1-1==index_2)
{
CheckBorder(index_2);
if(!_border[1])
return true;
}
return false;
}
/**
* @brief Create a priority queue containing sparse Matches
*
* This method computes the zncc for each Match extracted in "sparseMatching". If the zncc is
* over the correlation threshold then the Match is inserted in the output priority queue.
* @param[in] featuresLeft The feature locations in the left image.
* @param[in] featuresRight The features locations in the right image.
* @param[out] leftMap A matrix of points, of the same size as the left image. Each cell of this
* matrix stores the location of the corresponding point in the right image.
* @param[out] rightMap A matrix of points, the same size as the right image. Each cell of this
* matrix stores the location of the corresponding point in the left image.
* @return Priority queue containing sparse matches.
*/
t_matchPriorityQueue extractSparseSeeds(const std::vector< cv::Point2f > &featuresLeft,
const std::vector< cv::Point2f > &featuresRight,
cv::Mat_<cv::Point2i> &leftMap,
cv::Mat_<cv::Point2i> &rightMap)
{
t_matchPriorityQueue seeds;
for(uint i=0; i < featuresLeft.size(); i++)
{
// Calculate correlation and store match in Seeds.
Match m;
m.p0 = cv::Point2i(featuresLeft[i]);
m.p1 = cv::Point2i(featuresRight[i]);
m.corr = 0;
// Check if too close to boundary.
if(!CheckBorder(m,Param.borderX,Param.borderY, width, height))
continue;
m.corr = iZNCC_c1(m.p0, m.p1, Param.corrWinSizeX, Param.corrWinSizeY);
// Can we add it to the list
if( m.corr > Param.correlationThreshold )
{
seeds.push(m);
leftMap.at<cv::Point2i>(m.p0.y, m.p0.x) = m.p1;
rightMap.at<cv::Point2i>(m.p1.y, m.p1.x) = m.p0;
}
}
return seeds;
}
void GenerateShit::Update()
{
if(!isActive) return;
CheckBorder();
checkCollision();
checkDelete();
checkCreate();
}
//return true means has collide with obstacles
bool Map::CheckCollision(CIwFVec2 characterPos,CIwSVec2 characterBox,CIwFVec2 &target,CIwFVec2 characterPrePos)
{
//get border info for each _TileObstacles by checking certain tile's border info stored in tileset for each layer
//-1- get current character standing on tile index from the map
//check if character has moved to another tile since last frame, if not then don't update the rest
if(_characterPreIndex!=_characterIndex)
{
int index_Map[9];
//-2- get nearby 9 tiles index 0,1,2
// 3,4,5
// 6,7,8
index_Map[0]=_characterIndex-_width-1;
index_Map[1]=_characterIndex-_width;
index_Map[2]=_characterIndex-_width+1;
index_Map[3]=_characterIndex-1;
index_Map[4]=_characterIndex;
index_Map[5]=_characterIndex+1;
index_Map[6]=_characterIndex+_width-1;
index_Map[7]=_characterIndex+_width;
index_Map[8]=_characterIndex+_width+1;
//-3- get position for each nearby tiles
CIwFVec2 pos_Map[9];
pos_Map[4]=CIwFVec2(_index_Map_X*_tileWidth,_index_Map_Y*_tileHeight);
pos_Map[0]=CIwFVec2((_index_Map_X-1)*_tileWidth,(_index_Map_Y-1)*_tileHeight);
pos_Map[1]=CIwFVec2(_index_Map_X*_tileWidth,(_index_Map_Y-1)*_tileHeight);
pos_Map[2]=CIwFVec2((_index_Map_X+1)*_tileWidth,(_index_Map_Y-1)*_tileHeight);
pos_Map[3]=CIwFVec2((_index_Map_X-1)*_tileWidth,_index_Map_Y*_tileHeight);
pos_Map[5]=CIwFVec2((_index_Map_X+1)*_tileWidth,_index_Map_Y*_tileHeight);
pos_Map[6]=CIwFVec2((_index_Map_X-1)*_tileWidth,(_index_Map_Y+1)*_tileHeight);
pos_Map[7]=CIwFVec2(_index_Map_X*_tileWidth,(_index_Map_Y+1)*_tileHeight);
pos_Map[8]=CIwFVec2((_index_Map_X+1)*_tileWidth,(_index_Map_Y+1)*_tileHeight);
int index_Layer_Base,index_Layer_Middle,index_Layer_Maze;
for(int i=0;i!=9;i++)
{
CheckBorder(index_Map[i]);
_TileObstacles[i].UpdateObstacle(_border,pos_Map[i]);
if(_TileObstacles[i].CheckCollision(characterPos,characterBox,target,characterPrePos))
return true;
}
}
else
{
for(int i=0;i!=9;i++)
{
if(_TileObstacles[i].CheckCollision(characterPos,characterBox,target,characterPrePos))
return true;
}
}
return false;
}
bool Map::CheckTileConn(int index_1,int index_2)
{
CheckBorder(index_1);
//true means cannot pass
if(!_border[0]&&index_1-_width==index_2)
{
return true;
}
if(!_border[1]&&index_1+1==index_2)
{
return true;
}
if(!_border[2]&&index_1+_width==index_2)
{
return true;
}
if(!_border[3]&&index_1-1==index_2)
{
return true;
}
return false;
}
/**
* @brief Based on the seeds computed in sparse stereo, this method calculates the semi dense
* set of correspondences.
*
* The method initially discards low quality matches based on their zero-normalized cross
* correlation (zncc) value. This is done by calling the "extractSparseSeeds" method. Remaining
* high quality Matches stored in a t_matchPriorityQueue sorted according to their zncc value.
* The priority queue allows for new matches to be added while keeping track of the best Match.
* The algorithm then process the queue iteratively. In every iteration a Match is popped from
* the queue. The algorithm then tries to find candidate matches by matching every point in a
* small patch around the left Match feature, with a point within a same sized patch around the
* corresponding right feature. For each candidate point match, the zncc is computed and if it
* surpasses a threshold, the candidate pair is stored in a temporary priority queue. After this
* process completed the candidate matches are popped from the Local priority queue and if a
* match is not registered in refMap, it means that is the best match for this point. The
* algorithm registers this point in refMap and also push it to the Seed queue. If a candidate
* match is already registered, it means that is not the best and the algorithm discards it.
*
* @note This method does not have input arguments, but uses the "leftFeatures" and
* "rightFeatures" vectors.
* Also there is no output since the method used refMap and mtcMap to store the results.
* @param[in] featuresLeft The location of the features in the left image.
* @param[in] featuresRight The location of the features in the right image.
*/
void quasiDenseMatching(const std::vector< cv::Point2f > &featuresLeft,
const std::vector< cv::Point2f > &featuresRight)
{
dMatchesLen = 0;
refMap = cv::Mat_<cv::Point2i>(cv::Size(width, height), cv::Point2i(0, 0));
mtcMap = cv::Point2i(0, 0);
// build texture homogeneity reference maps.
buildTextureDescriptor(grayLeft, textureDescLeft);
buildTextureDescriptor(grayRight, textureDescRight);
// generate the intergal images for fast variable window correlation calculations
cv::integral(grayLeft, sum0, ssum0);
cv::integral(grayRight, sum1, ssum1);
// Seed priority queue. The algorithm wants to pop the best seed available in order to densify
//the sparse set.
t_matchPriorityQueue seeds = extractSparseSeeds(featuresLeft, featuresRight,
refMap, mtcMap);
// Do the propagation part
while(!seeds.empty())
{
t_matchPriorityQueue Local;
// Get the best seed at the moment
Match m = seeds.top();
seeds.pop();
// Ignore the border
if(!CheckBorder(m, Param.borderX, Param.borderY, width, height))
continue;
// For all neighbours of the seed in image 1
//the neighborghoud is defined with Param.N*2 dimentrion
for(int y=-Param.neighborhoodSize;y<=Param.neighborhoodSize;y++)
{
for(int x=-Param.neighborhoodSize;x<=Param.neighborhoodSize;x++)
{
cv::Point2i p0 = cv::Point2i(m.p0.x+x,m.p0.y+y);
// Check if its unique in ref
if(refMap.at<cv::Point2i>(p0.y,p0.x) != NO_MATCH)
continue;
// Check the texture descriptor for a boundary
if(textureDescLeft.at<int>(p0.y, p0.x) > Param.textrureThreshold)
continue;
// For all candidate matches.
for(int wy=-Param.disparityGradient; wy<=Param.disparityGradient; wy++)
{
for(int wx=-Param.disparityGradient; wx<=Param.disparityGradient; wx++)
{
cv::Point p1 = cv::Point(m.p1.x+x+wx,m.p1.y+y+wy);
// Check if its unique in ref
if(mtcMap.at<cv::Point2i>(p1.y, p1.x) != NO_MATCH)
continue;
// Check the texture descriptor for a boundary
if(textureDescRight.at<int>(p1.y, p1.x) > Param.textrureThreshold)
continue;
// Calculate ZNCC and store local match.
float corr = iZNCC_c1(p0,p1,Param.corrWinSizeX,Param.corrWinSizeY);
// push back if this is valid match
if( corr > Param.correlationThreshold )
{
Match nm;
nm.p0 = p0;
nm.p1 = p1;
nm.corr = corr;
Local.push(nm);
}
}
}
}
}
// Get seeds from the local
while( !Local.empty() )
{
Match lm = Local.top();
Local.pop();
// Check if its unique in both ref and dst.
if(refMap.at<cv::Point2i>(lm.p0.y, lm.p0.x) != NO_MATCH)
continue;
if(mtcMap.at<cv::Point2i>(lm.p1.y, lm.p1.x) != NO_MATCH)
continue;
// Unique match
refMap.at<cv::Point2i>(lm.p0.y, lm.p0.x) = lm.p1;
mtcMap.at<cv::Point2i>(lm.p1.y, lm.p1.x) = lm.p0;
dMatchesLen++;
// Add to the seed list
seeds.push(lm);
}
}
}
bool Map::CheckMazePath()
{
IW_CALLSTACK("MAP::CHECKMAZEPATH()");
if(!CheckTileConn(mazeStartIndex[mazeFinished],mapStartIndex[mazeFinished]))
return false;
_path->Init(mazeStartIndex[mazeFinished],mazeEndIndex[mazeFinished]);
CIwArray<int> openNodes,closeNodes;
openNodes.append(mazeStartIndex[mazeFinished]);
for(int i=0;i!=openNodes.size();i++)
{
int curIndex=openNodes[i];
if(closeNodes.contains(curIndex))
continue;
CheckBorder(curIndex);
int index_Layer_Maze;
bool bor[4];
memcpy(bor,_border,sizeof(_border));
IW_CALLSTACK("MAP::CHECKMAZEPATH()-memcpy");
for(int j=0;j!=4;j++)
{
//true means cannot pass
if(!bor[j])
{
int ind=curIndex;
switch(j)
{
case 0:
ind=curIndex-_width;break;
case 1:
ind=curIndex+1;break;
case 2:
ind=curIndex+_width;break;
case 3:
ind=curIndex-1;break;
default:
ind=curIndex;break;
}
index_Layer_Maze=_layer_maze->m_TileIndex[ind]-_tileset_maze->m_firstGid;
IW_CALLSTACK("MAP::CHECKMAZEPATH()-index_Layer_Maze");
if(index_Layer_Maze>=0)
{
if(CheckMazeTileConn(curIndex,j,ind))
{
_path->AddPathNode(ind,curIndex);
if(!openNodes.contains(ind))
openNodes.append(ind);
}
}
}
}
closeNodes.append(openNodes[i]);
if(_path->isEnd())
{
if(CheckTileConn(mazeEndIndex[mazeFinished],mapEndIndex[mazeFinished]))
{
mazeFinished++;
return true;
}
}
}
return false;
}
