void MatchSet :: add_match( const PointT point_c, const PointT point_r, float strength, int ref_idx ){
Match match;
match.point_c = PointT(point_c);
match.point_r = PointT(point_r);
match.strength = strength;
match.ref_idx = ref_idx;
_matches.push_back(match);
}
void MatchSet :: get_matches( vector<Match> &matches ) const{
for (unsigned int i = 0; i < _matches.size(); i++ ){
Match match;
match.point_c = PointT(_matches[i].point_c);
match.point_r = PointT(_matches[i].point_r);
match.strength = _matches[i].strength;
match.ref_idx = _matches[i].ref_idx;
matches.push_back(match);
}
}
void RedlineExtractorNode::processImage(void)
{
cv::Vec3b color;
double excessRedValue;
// Reset points of interest point cloud and image
this->poiCloud.clear();
this->poiImage = cv::Mat::zeros(this->input.cvImage->image.rows, this->input.cvImage->image.cols, CV_8UC3);
// Iterate through pixels of image (TODO: Faster method exists)
for (int i = 0; i < this->input.cvImage->image.rows; i++)
{
for (int j = 0; j < this->input.cvImage->image.cols; j++)
{
color = this->input.cvImage->image.at<cv::Vec3b>(i,j); // Get color of current pixel
excessRedValue = this->params.redWeight * color.val[2] -
this->params.greenWeight * color.val[1] -
this->params.blueWeight * color.val[0]; // Calculate excess red value
if (excessRedValue > this->params.threshold)
{
this->poiImage.at<cv::Vec3b>(i,j) = cv::Vec3b(255, 255, 255); // Assign white to the pixel of interest
// TODO: Homography
this->poiCloud.push_back(PointT((double)j,(double)i,0.0f)); // Add interest point to the point cloud
}
}
}
}
Extractors::RowExtractor::RowExtractor()
{
// Setup default parameters
this->systemParameters.timeLimit = 1000;
// Pre-Processor
this->systemParameters.preProcessor.active = false;
this->systemParameters.preProcessor.minimumClearZoneDistance = 0.0;
this->systemParameters.preProcessor.maximumClearZoneDistance = 0.0;
// Ransac processor
this->systemParameters.ransacProcessor.numberOfRansacTries = 0;
this->systemParameters.ransacProcessor.numberOfPointsToAcceptLine = 0;
this->systemParameters.ransacProcessor.distanceFromLineThreshold = 0.0;
this->systemParameters.ransacProcessor.minimumRowLength = 0.0;
// Setup input (empty)
this->input.time_sec = 0;
this->input.time_nsec = 0;
this->input.pointCloud = pcl::PointCloud<PointT>();
// Setup output (empty)
this->output.rowFound = false;
this->output.length = 0.0;
this->output.orientation = 0.0;
this->output.center = PointT();
}
static PointT apply(PointT const & p1,
PointT const & p2)
{
return PointT(p1[1]*p2[2]-p1[2]*p2[1],
p1[2]*p2[0]-p1[0]*p2[2],
p1[0]*p2[1]-p1[1]*p2[0]);
}
void Extractors::RowExtractor::rowProcessor (void)
{
// Variables
const bool debug = false;
const double lowerBoundPercentage = 0.1;
double totalSizeOfCloud = (double)this->preProcessedData.size();
double percentageLeft = 1.0;
Row ransacOutput;
// Clear output
this->output.rows.clear();
this->output.rowFound = false;
// Find row(s) in point cloud with ransac
while (percentageLeft > lowerBoundPercentage)
{
ransacOutput = ransacProcessor();
this->output.rows.push_back(ransacOutput);
percentageLeft = (double)this->preProcessedData.size() / totalSizeOfCloud;
}
// Process row(s) and finalize output
int tempCount = 0;
double meanOrientation;
for (int i = 0; i < this->output.rows.size(); i++)
{
this->output.rowFound |= this->output.rows[i].rowFound;
if (this->output.rows[i].rowFound)
{
meanOrientation += this->output.orientation;
tempCount++;
}
}
if (tempCount)
{
this->output.rowFound = true;
meanOrientation /= (double)tempCount;
this->output.orientation = meanOrientation;
this->output.length = 0.0;
this->output.center.x = 0;
this->output.center.y = 0;
}
else
{
this->output.rowFound = false;
this->output.orientation = giveMeNan();
this->output.length = giveMeNan();
this->output.center = PointT();
}
}
Extractors::RowExtractor::RowExtractor(Parameters par)
{
// Setup default parameters
this->systemParameters = par;
// Setup input (empty)
this->input.time_sec = 0;
this->input.time_nsec = 0;
this->input.pointCloud = pcl::PointCloud<PointT>();
// Setup output (empty)
this->output.rowFound = false;
this->output.length = 0.0;
this->output.orientation = 0.0;
this->output.center = PointT();
}
PointT operator-(){
return PointT(-x, -y);
}
PointT operator-(PointT pt){
return PointT(x - pt.x, y - pt.y);
}
PointT operator+(PointT pt){
return PointT(x + pt.x, y + pt.y);
}
PointT operator/(T coef){
return PointT(x / coef, y / coef);
}
PointT operator*(T coef){
return PointT(x * coef, y * coef);
}
static PointT apply(PointT,
PointT)
{
return PointT(0,0);
}
PointT cw(){
return PointT(y, -x);
}
Extractors::Row Extractors::RowExtractor::ransacProcessor (void)
{
const bool debug = false;
// Setup empty row
Row row;
row.rowFound = false;
row.pointInRow = PointT();
row.lengthToRow = 0.0;
row.orientation = 0.0;
row.varianceOfRes = 0.0;
if (debug) printf(" Ransac processing:\n=========================================\n");
// If points available in cloud, process them
if (this->preProcessedData.size())
{
if (debug) printf(" Processing %d points in cloud\n", (int)this->preProcessedData.size());
// Seed randomizer
srand(time(0));
// Variables
int ransacTryCounter = 0; // Counter for ransac tries
int randomPoints[2]; // Random index from cloud
int randomTryCounter; // Counter of the number of random points selected
const int maxRandomCount = 10; // Maximum number of random tries (NB not ransac tries)
PointT lookingAt[2]; // Currently looking at points
PointT pointsOfInterest[2]; // Points selected for further analysis
PointT lineDirectionVector; // Direction vector for line (vector between points of interest)
double lineLength; // Length of direction vector
double lineRotation; // Line rotation
bool lineFound; // Did ransac select points far enough away from each other
bool rowFound = false; // Row found?
PointT currentPoint; // Current point to test
PointT currentVector; // Current vector (from lookingAt[0] to currentPoint)
PointT orthogonalVector; // Vector perpendicular to direction vector
PointT xProjection; // Projection of currentVector onto lineDirectionVector
PointT yProjection; // Projection of currentVector onto orthogonalVector
double xDotProduct; // Dot product between currentVector and lineDirectionVector
double yDotProduct; // Dot product between currentVector and orthogonalVector
double currentVecLen; // Length of current vector
double orthoVecLen; // Length of orthogonal vector
double xProjLen; // Length of projection of currentVector onto lineDirectionVector
double yProjLen; // Length of projection of currentVector onto orthogonalVector
int ransacInlierCounter; // Count inlier points
int ransacBestCount = 0; // Best inlier ransac count
std::list<double> xProjTestDist; // Tentative placeholder of length of vectors projected onto line
std::list<double> xProjDistribution; // Store projection length
std::list<double> weights; // Store weights from xProjDistribution
std::list<double> residuals; // Store residuals from linear regression on CDF of points along the line
std::vector<int> inlierIndecies; // Storing indecies of inlier points
inlierIndecies = std::vector<int>();
double inliersPercentage; // Percentage of inliers compared to the total number of points in the pre-processed cloud
// Find possible row
while (ransacTryCounter < this->systemParameters.ransacProcessor.numberOfRansacTries)
{
ransacTryCounter++;
inlierIndecies.clear();
ransacBestCount = 0;
randomTryCounter = 0;
lineFound = false;
do
{
randomPoints[0] = rand() % this->preProcessedData.size();
randomPoints[1] = rand() % this->preProcessedData.size();
lookingAt[0] = this->preProcessedData.at(randomPoints[0]);
lookingAt[1] = this->preProcessedData.at(randomPoints[1]);
// Calculate direction from two randomly selected points
lineDirectionVector.x = lookingAt[1].x - lookingAt[0].x;
lineDirectionVector.y = lookingAt[1].y - lookingAt[0].y;
lineLength = sqrt(pow(lineDirectionVector.x, 2) + pow(lineDirectionVector.y, 2));
randomTryCounter++;
if (lineLength >= this->systemParameters.ransacProcessor.minimumRowLength) lineFound = true;
} while (!lineFound && randomTryCounter < maxRandomCount);
// If proper line is found continue further analysis
if (lineFound)
{
ransacInlierCounter = 0;
xProjTestDist.clear();
weights.clear();
orthogonalVector.x = - lineDirectionVector.y;
orthogonalVector.y = lineDirectionVector.x;
// Traverse all points in cloud
for (int i = 0; i < this->preProcessedData.size(); i++)
{
// Get current point
currentPoint = this->preProcessedData[i];
// Calculate "TLS" distance
currentVector.x = currentPoint.x - lookingAt[0].x;
currentVector.y = currentPoint.y - lookingAt[0].y;
// Make projection of currentVector onto directionVector and orthogonalVector
xDotProduct = currentVector.x * lineDirectionVector.x + currentVector.y * lineDirectionVector.y;
yDotProduct = currentVector.x * orthogonalVector.x + currentVector.y * orthogonalVector.y;
currentVecLen = sqrt(pow(currentVector.x, 2) + pow(currentVector.y, 2));
orthoVecLen = sqrt(pow(orthogonalVector.x, 2) + pow(orthogonalVector.y, 2));
xProjection.x = xDotProduct / pow(currentVecLen, 2) * currentVector.x;
xProjection.y = xDotProduct / pow(currentVecLen, 2) * currentVector.y;
yProjection.x = yDotProduct / pow(orthoVecLen, 2) * orthogonalVector.x;
yProjection.y = yDotProduct / pow(orthoVecLen, 2) * orthogonalVector.y;
xProjLen = sqrt(pow(xProjection.x, 2) + pow(xProjection.y, 2));
yProjLen = sqrt(pow(yProjection.x, 2) + pow(yProjection.y, 2));
// If it is an inlier
if (yProjLen < this->systemParameters.ransacProcessor.distanceFromLineThreshold)
{
ransacInlierCounter++;
// Add index to inlier index
inlierIndecies.push_back(i);
// If projection onto direction vector is in bound add length to distribution
if (xDotProduct > 0 && xProjLen < lineLength)
{
xProjTestDist.push_back(xProjLen);
}
}
}
if (ransacInlierCounter > ransacBestCount)
{
ransacBestCount = ransacInlierCounter;
xProjDistribution = xProjTestDist;
}
}
}
// If any "best" line was found, analyse quality
if (xProjDistribution.size())
{
// Calculate the percentage of inliers to line compared with the overall number of points
inliersPercentage = (double)xProjDistribution.size() / (double)this->preProcessedData.size();
xProjDistribution.sort();
if (debug)
{
printf(" Number of inliers: %d\n", (int)inlierIndecies.size());
printf(" Before reduction of point cloud: %d\n", (int)this->preProcessedData.size());
}
// Remove inliers from pre-processed cloud
for (int index = inlierIndecies.size() - 1; index >= 0; index--)
{
this->preProcessedData.erase(preProcessedData.begin() + inlierIndecies[index]);
}
if (debug) printf(" After reduction of point cloud: %d\n", (int)this->preProcessedData.size());
double numberOfPoints = (double)xProjDistribution.size();
double weight = 1. / numberOfPoints;
double cumulativeWeight = 0.0;
for (std::list<double>::iterator it = xProjDistribution.begin(); it != xProjDistribution.end(); ++it)
{
cumulativeWeight += weight;
weights.push_back(cumulativeWeight);
}
// Check distribution
double sumX = 0, sumY = 0, sumXY = 0, sumSquareX = 0;
double meanX, meanY, meanXY, meanSquareX;
double alpha, beta;
for (std::list<double>::iterator itx = xProjDistribution.begin(), ity = weights.begin(); itx != xProjDistribution.end(); ++itx, ++ity)
{
sumX += *itx;
sumY += *ity;
sumXY += (*itx) * (*ity);
sumSquareX += pow(*itx, 2);
}
meanX = sumX / numberOfPoints;
meanY = sumY / numberOfPoints;
meanXY = sumXY / numberOfPoints;
meanSquareX = sumSquareX / numberOfPoints;
// Calculate linear regression parameters
alpha = (meanXY - meanX * meanY) / (meanSquareX - pow(meanX, 2));
beta = meanY - alpha * meanX;
// Calculate variance of residuals
double residual, sumRes = 0, sumResSquared = 0, variance;
for (std::list<double>::iterator itx = xProjDistribution.begin(), ity = weights.begin(); ity != weights.end(); ++itx, ++ity)
{
residual = *ity - (alpha * (*itx) + beta);
sumRes += residual;
sumResSquared += pow(residual, 2);
}
variance = (sumResSquared - pow(sumRes, 2)) / numberOfPoints;
lineRotation = std::atan2(lineDirectionVector.y, lineDirectionVector.x);
// Setup output
row.rowFound = true;
row.pointInRow = lookingAt[0];
row.lengthToRow = lineLength;
row.orientation = lineRotation;
row.varianceOfRes = variance;
if (debug)
{
printf(" Point in line: (%f, %f)\n", lookingAt[0].x, lookingAt[0].y);
printf(" Line length: %f\n", lineLength);
printf(" Line rotation: %f\n", lineRotation);
printf(" Variance of residuals: %f\n", variance);
}
}
else
{
if (debug) printf(" No fit was made from point cloud!\n");
}
}
else
{
if (debug) printf(" No points in point cloud!\n");
}
if (debug) printf("=========================================\n Ransac processing ended\n\n\n");
return row;
}
std::vector<std::map<std::pair<int, int>, std::string> > Map::analyzeFlowMap(){
bool found = false;
// std::vector<std::map<std::pair<int, int>, std::string> > solution = analyzeDotPair(DotsMap.begin(), initMapInfo, found);
std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
std::map<std::string, DotPairT*>::iterator map_it = DotsMap.begin();
PointT start = map_it->second->loc1;
PointT end = map_it->second->loc2;
std::queue<PathNodeT*> BFS;
std::map<std::pair<int, int>, std::string> tmpMap;
std::map<std::pair<int, int>, std::string> curMapInfo = initMapInfo;
PathNodeT* curPathNode = new PathNodeT;
PointT curPoint;
PointT nextPoint;
curPathNode->prev = NULL;
curPathNode->cur = start;
curPathNode->pathMap = curMapInfo;
BFS.push(curPathNode);
int count1=0;
while (!BFS.empty()){
curPathNode = BFS.front();
curPoint = curPathNode->cur;
tmpMap = curPathNode->pathMap;
BFS.pop();
/*
if (tmpMap.size() > size * size * factor){
delete curPathNode;
break;
}
*/
/*
if (curPoint.x == end.x && curPoint.y == end.y){
pathList->push_back(curPathNode);
continue;
}
*/
nextPoint =PointT(curPoint.x - 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
std::cout<<++count1<<map_it->first<<std::endl;
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x + 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
std::cout<<++count1<<map_it->first<<std::endl;
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y - 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
std::cout<<++count1<<map_it->first<<std::endl;
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y + 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
std::cout<<++count1<<map_it->first<<std::endl;
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
delete curPathNode;
}
// Add one path to map and analyze next dot pair
std::vector<std::map<std::pair<int, int>, std::string> > solutions;
// for (std::vector<PathNodeT*>::iterator it = pathList->begin(); it != pathList->end(); ++it) {
omp_lock_t lock;
omp_init_lock(&lock);
int count = 0;
#pragma omp parallel for schedule(dynamic)
for(int i = 0; i < pathList->size(); ++i){
//nextMapInfo = (*it)->pathMap;
if (!found){
std::map<std::pair<int, int>, std::string> nextMapInfo;
std::vector<std::map<std::pair<int, int>, std::string> > tmpSolutions;
PathNodeT* tmp;
nextMapInfo = pathList->at(i)->pathMap;
/*
tmp = *it;
while(tmp){
nextMapInfo.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(tmp->cur.x, tmp->cur.y), map_it->first));
//nextColorMap.push_back(DotT(map_it->first, tmp->cur));
tmp = tmp->prev;
}*/
std::map<std::string, DotPairT*>::iterator tmpIt = map_it;
++tmpIt;
if (tmpIt == DotsMap.end()){
omp_set_lock(&lock);
solutions.push_back(nextMapInfo);
omp_unset_lock(&lock);
/* Modified */
found = true;
//break;
//return solutions;
/* Modified */
}
else{
tmpSolutions = analyzeDotPair(tmpIt, nextMapInfo, found);// Modified
omp_set_lock(&lock);
solutions.insert(solutions.end(), tmpSolutions.begin(), tmpSolutions.end());
omp_unset_lock(&lock);
//if(!found)
//std::cout<<++count<<"\t"<<pathList->size()<<std::endl;
/* Modified */
//if (found){
// break;
//}
}
}
}
return solutions;
}
void Map::generatePathsForPair(std::string color){
std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
PointT start = DotsMap[color]->loc1;
PointT end = DotsMap[color]->loc2;
std::queue<PathNodeT*> BFS;
std::map<std::pair<int, int>, std::string> tmpMap;
PathNodeT* curPathNode = new PathNodeT;
PointT curPoint;
PointT nextPoint;
int count = 0;
curPathNode->prev = NULL;
curPathNode->cur = start;
curPathNode->pathMap = initMapInfo;
BFS.push(curPathNode);
while (!BFS.empty()){
curPathNode = BFS.front();
curPoint = curPathNode->cur;
tmpMap = curPathNode->pathMap;
BFS.pop();
if (tmpMap.size()> size*size*factor){
delete curPathNode;
break;
}
/*
if (curPoint.x == end.x && curPoint.y == end.y){
pathList->push_back(curPathNode);
continue;
}
*/
nextPoint =PointT(curPoint.x - 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, tmpMap)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x + 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, tmpMap)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y - 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, tmpMap)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y + 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, tmpMap)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
delete curPathNode;
}
std::vector<MapInfo> tmp;
for (std::vector<PathNodeT*>::iterator it = pathList->begin();
it != pathList->end();
++it)
tmp.push_back((*it)->pathMap);
possiblePaths[color] = pathList;
return;
}
std::vector<std::map<std::pair<int, int>, std::string> > Map::analyzeDotPair(std::map<std::string, DotPairT*>::iterator map_it, std::map<std::pair<int, int>, std::string> curMapInfo, bool &found){
if (found){
std::vector<std::map<std::pair<int, int>, std::string> > something;
return something;
}
std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
PointT start = map_it->second->loc1;
PointT end = map_it->second->loc2;
std::queue<PathNodeT*> BFS;
std::map<std::pair<int, int>, std::string> tmpMap;
PathNodeT* curPathNode = new PathNodeT;
PointT curPoint;
PointT nextPoint;
curPathNode->prev = NULL;
curPathNode->cur = start;
curPathNode->pathMap = curMapInfo;
BFS.push(curPathNode);
while (!BFS.empty()){
curPathNode = BFS.front();
curPoint = curPathNode->cur;
tmpMap = curPathNode->pathMap;
BFS.pop();
/*
if ((tmpMap.size() - curMapInfo.size()) >= (size * size - curMapInfo.size()) * factor){
delete curPathNode;
break;
}
*/
/*
if (curPoint.x == end.x && curPoint.y == end.y){
pathList->push_back(curPathNode);
continue;
}
*/
nextPoint =PointT(curPoint.x - 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x + 1, curPoint.y);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y - 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
nextPoint =PointT(curPoint.x, curPoint.y + 1);
if (nextPoint.x == end.x && nextPoint.y == end.y){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
continue;
}
if (!isCollide(curPathNode, nextPoint, curMapInfo)){
tmpMap = curPathNode->pathMap;
tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
}
delete curPathNode;
}
// Add one path to map and analyze next dot pair
std::map<std::pair<int, int>, std::string> nextMapInfo;
std::vector<std::map<std::pair<int, int>, std::string> > solutions;
std::vector<std::map<std::pair<int, int>, std::string> > tmpSolutions;
PathNodeT* tmp;
for (std::vector<PathNodeT*>::iterator it = pathList->begin(); it != pathList->end(); ++it) {
nextMapInfo = (*it)->pathMap;
/*
tmp = *it;
while(tmp){
nextMapInfo.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(tmp->cur.x, tmp->cur.y), map_it->first));
//nextColorMap.push_back(DotT(map_it->first, tmp->cur));
tmp = tmp->prev;
}*/
std::map<std::string, DotPairT*>::iterator tmpIt = map_it;
++tmpIt;
if (tmpIt == DotsMap.end()){
solutions.push_back(nextMapInfo);
/* Modified */
found = true;
return solutions;
/* Modified */
}
else{
tmpSolutions = analyzeDotPair(tmpIt, nextMapInfo, found);// Modified
/* Modified */
if (found){
return tmpSolutions;
}
solutions.insert(solutions.end(), tmpSolutions.begin(), tmpSolutions.end());
}
}
//std::cout<<"pass one "<<map_it->first<<std::endl;
return solutions;
}
PointT ccw(){
return PointT(-y, x);
}
static PointT apply(PointT const &,
PointT const &)
{
return PointT(0);
}
