void BasicVFF::calcRepulsiveForceSum(const GridMap &map) { for(int i = 0; i < map.getMapSize().y; i++) { for(int j = 0; j < map.getMapSize().x; j++) { uchar occupancyData = map.getOccupancyGridMap()[i * map.getMapSize().x + j]; if(occupancyData > 0) { Position2f ObstaclePos; map.getPositionFromIndex(Cell2i(i,j), ObstaclePos); //cout << i << " " << j << endl; //cout << ObstaclePos.x << " " << ObstaclePos.y << endl; float distancePow = getDistancePow(ObstaclePos); float distance = powf(distancePow, 0.5); force2f force = force2f((0 - ObstaclePos.x) / distance , (0 - ObstaclePos.y) / distance); float coeff = (float)occupancyData / distancePow; //cout << (float)occupancyData << " "<< distancePow << " " << coeff<< endl; force2f force1 = coeff * force; forceSum += force1; //cout << force.x << " " << force.y << endl; //cout << force1.x << " " << force1.y << endl; } } } cout << "ForceSum = " << forceSum.x << " "<< forceSum.y << endl; }
void BasicVFF::calcDistanceVecArray(const GridMap &map) { for(int i = 0; i < map.getMapSize().y; i++) { for(int j = 0; j < map.getMapSize().x; j++) { if (map.getOccupancyGridMap()[i * map.getMapSize().x + j] > 0) { Position2f ObstaclePos; map.getPositionFromIndex(Cell2i(i,j), ObstaclePos); cout << i << " " << j << endl; cout << ObstaclePos.x << " " << ObstaclePos.y << endl; distance_vec_array.push_back(ObstaclePos); cout << distance_vec_array.size() << endl; } } } }