/**
* The actual Recursive Elastic Matching function
* See my thesis for some nicely printed equations
**/
int
D( int i, int j )
{
if ( lookup[i][j] != -1 )
return lookup[i][j];
if ( j == 1 )
return lookup[i][j] = pointDistance(i, 1)
+ min(D(i-1, 1), D(i-1, 0));
else
return lookup[i][j] = pointDistance(i,j)
+ min(D(i-1, j), min(D(i-1, j-1), D(i-1, j-2)));
}
void FighterFightTask::updateState() {
if (m_state != State::IDLE && m_targets.empty()) {
setState(State::IDLE);
}
switch (m_state) {
case State::IDLE:
if (m_targets.empty()) {
return;
} else {
setState(State::APPROACH);
}
break;
case State::APPROACH:
if (targetDistance() < componentsInfo().maxBulletRange()) {
setState(State::ENGAGE);
}
break;
case State::ENGAGE:
if (targetDistance() < m_minEnemyDistance) {
setState(State::EVADE);
m_positionBehindTarget = findRandomEvasionPoint();
}
break;
case State::EVADE:
if (pointDistance(m_positionBehindTarget) < 20 && targetDistance() < m_minEnemyDistance) {
m_positionBehindTarget = findRandomEvasionPoint();
}
if (pointDistance(m_positionBehindTarget) > 20)
break;
if (targetDistance() < m_minEnemyDistance) {
break;
}
if (targetDistance() < componentsInfo().maxBulletRange()) {
setState(State::ENGAGE);
} else {
setState(State::APPROACH);
}
break;
default:
assert(false);
return;
}
if (m_stateChanged) {
m_stateChanged = false;
updateState();
}
}
void actionDetector::coolDownDetect(std::list<Point> m_History, Point initPoint)
{
// coolDownDetect only operate when m_History.size() >= coolDownConNum + 1 (need coolDownConNum distance less than the coolDownThreshold
//if (m_History.size() < coolDownConNum + 1)
// printf("coolDownDetect should not be operated when m_History.size() < 15);
int m_HistorySize = m_History.size();
std::list<int> isMove;
std::list<int>::iterator iter_isMove = isMove.begin();
Point current, before;
double distance;
// count to control how many consecutive points we really need
int count = 0;
// isCoolDown Default is false.
bool isCoolDown = false;
for (std::list<Point>::iterator iter_m_History = m_History.begin(); count < coolDownConNum; iter_m_History++)
{
current = assignPoint(iter_m_History);
distance = pointDistance(current, initPoint);
if (distance < coolDownThreshold)
isMove.push_front(1);
else
isMove.push_front(0);
count++;
}
int sumList = std::accumulate(isMove.begin(), isMove.end(), 0);
if (sumList == coolDownConNum){
isCoolDown = true;
printf("Cool Down Detected");}
}
/**
* @brief AgentCluster::moveTowards Moves the first agent towards the second one.
* @param agentOne The agent being moved.
* @param agentTwo The agent who is being moved towards.
*/
void AgentCluster::moveTowards(Agent *agentOne, Agent *agentTwo) {
double crowdingFactor = (agentOne->crowdingRange + agentTwo->crowdingRange) * 0.5;
double agentDistance = pointDistance(agentOne->x, agentTwo->x, agentOne->y, agentTwo->y) - crowdingFactor;
if (agentDistance == 0)
return;
double moveMagnitude = std::min(randomDouble(0, agentOne->foragingRange), agentDistance) * randomDouble(0, 0.9);
double unitVectorX = (agentTwo->x - agentOne->x) / agentDistance;
double unitVectorY = (agentTwo->y - agentOne->y) / agentDistance;
double newX = agentOne->x + (moveMagnitude * unitVectorX);
double newY = agentOne->y + (moveMagnitude * unitVectorY);
Q_ASSERT_X(nanTest(newX), "Failed NaN", __FUNCTION__);
Q_ASSERT_X(nanTest(newY), "Failed NaN", __FUNCTION__);
if (newX > m_dataMaxX)
newX = m_dataMaxX;
else if (newX < m_dataMinX)
newX = m_dataMinX;
if (newY > m_dataMaxY)
newY = m_dataMaxY;
else if (newY < m_dataMinY)
newY = m_dataMinY;
agentOne->x = newX;
agentOne->y = newY;
agentOne->happiness = calculateHappiness(agentOne);
}
double calPolylineDist(const vector<DjiPoint>& pts)
{
double pathCost = 0;
for (int i = 0; i < (int)pts.size() - 1; i++)
{
pathCost += pointDistance(pts[i], pts[i + 1]);
}
return pathCost;
}
/**
* @brief AgentCluster::dataWithinForagingRange Finds data points within the give Agent's foraging
* range.
* @param agent The Agent to find data points for.
* @return A vector of ClusterItem objects within the Agent's foraging range.
*/
std::vector<ClusterItem*> AgentCluster::dataWithinForagingRange(Agent *agent) const {
std::vector<ClusterItem*> items;
for (unsigned int i = 0; i < m_data.size(); i++) {
ClusterItem* item = m_data[i];
double distance = pointDistance(agent->x, item->x, agent->y, item->y);
if (distance <= agent->foragingRange)
items.push_back(item);
}
return items;
}
double gen_radius(Point p, Box *curr, Box **qtree){
double ulDist, urDist, llDist, lrDist, maxDist;
Point ul_corner, lr_corner;
Box *b;
b = qtree[curr->parent];
ul_corner.x = b->ll_corner.x;
ul_corner.y = b->ur_corner.y;
lr_corner.x = b->ur_corner.x;
lr_corner.y = b->ll_corner.y;
ulDist = pointDistance(p, ul_corner);
urDist = pointDistance(p, b->ur_corner);
llDist = pointDistance(p, b->ll_corner);
lrDist = pointDistance(p, lr_corner);
return fmax(ulDist, fmax(urDist, fmax(llDist, lrDist)));
}
double DistanceTransform::distance2Edge(const Point3f& pnt, const Point3f& v1, const Point3f& v2, Point3f& ne)
{
vector3 edge(v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]);
float len = edge.length();
edge.normalize();
vector3 vec(pnt[0]-v1[0], pnt[1]-v1[1], pnt[2]-v1[2]);
float dot = DotProduct(vec, edge);
if(dot < 0) {
ne = v1;
return pointDistance(pnt, v1);
} else if(dot > len) {
ne = v2;
return pointDistance(pnt, v2);
}
ne[0] = v1[0] + dot * edge[0];
ne[1] = v1[1] + dot * edge[1];
ne[2] = v1[2] + dot * edge[2];
return pointDistance(pnt, ne);
}
/**
* newLookup
* Initializes the lookup-table
**/
void
newLookup( int n, int m )
{
int i, j;
if ( lookup != NULL )
freeLookup();
lookup = (int **) malloc( (lookup_n = n) * sizeof(int *));
for ( i = 0; i < lookup_n; i++ )
{
lookup[i] = (int *) malloc( m * sizeof(int) );
for ( j = 0; j < m; j++ )
lookup[i][j] = -1;
}
lookup[0][0] = pointDistance(0, 0);
for ( j = 1; j < m; j++ )
lookup[0][j] = lookup[0][j-1] + pointDistance(0, j);
for ( i = 1; i < n; i++ )
lookup[i][0] = lookup[i-1][0] + pointDistance(i, 0);
}
MonoPointDistortionMeshDescription
UnstructuredMeshInterpolator::getNearestPoints(
float xN, float yN, const MonoPointDistortionMeshDescription& points) {
MonoPointDistortionMeshDescription ret;
// Find the three non-collinear points in the mesh that are nearest
// to the normalized point we are trying to look up. We start by
// sorting the points based on distance from our location, selecting
// the first two, and then looking through the rest until we find
// one that is not collinear with the first two (normalized dot
// product magnitude far enough from 1). If we don't find such
// points, we just go with the values from the closest point.
typedef std::multimap<double, size_t> PointDistanceIndexMap;
PointDistanceIndexMap map;
//if (points.size() < 460 && points.size() > 0)
// std::cout << "XXX Sorting mesh of size " << points.size() << std::endl;
for (size_t i = 0; i < points.size(); i++) {
// Insertion into the multimap sorts them by distance.
map.insert(std::make_pair(
pointDistance(xN, yN, points[i][0][0], points[i][0][1]), i));
}
PointDistanceIndexMap::const_iterator it = map.begin();
size_t first = it->second;
it++;
size_t second = it->second;
it++;
size_t third = first;
while (it != map.end()) {
if (!nearly_collinear(points[first][0], points[second][0],
points[it->second][0])) {
third = it->second;
break;
}
it++;
}
// Push back all of the points we found, which may not include
// a third point if the first is the same as the third.
if (map.size() >= 1) {
ret.push_back(points[first]);
}
if (map.size() >= 2) {
ret.push_back(points[second]);
}
if (third != first) {
ret.push_back(points[third]);
}
return ret;
}
/**
* @brief AgentCluster::assignmentPhase Runs the assignment phase of the AgentSwarm algorithm.
*/
void AgentCluster::assignmentPhase() {
for (unsigned int i = 0; i < m_agents.size(); i++) {
Agent* agent = m_agents[i];
if (agent->visited)
continue;
agent->visited = true;
Cluster* cluster = new Cluster();
cluster->id = m_clusters.size();
m_clusters.push_back(cluster);
std::vector<Agent*> neighbors = agentsWithinRange(agent, agent->foragingRange * 2.0);
addToCluster(cluster, agent, neighbors);
}
for (unsigned int i = 0; i < m_clusters.size(); i++) {
Cluster* cluster = m_clusters[i];
for (unsigned int j = 0; j < cluster->agents.size(); j++) {
Agent* agent = cluster->agents[j];
std::vector<ClusterItem*> items = dataWithinForagingRange(agent);
for (unsigned int k = 0; k < items.size(); k++) {
ClusterItem* item = items[k];
if (item->group != -1)
continue;
item->group = cluster->id;
cluster->points.push_back(item);
}
}
}
for (unsigned int i = 0; i < m_data.size(); i++) { //assign unassigned points to the closest groups
ClusterItem* item = m_data[i];
if (item->group != -1)
continue;
Agent* closestAgent = 0;
double closestDistance = -1;
for (unsigned int j = 0; j < m_agents.size(); j++) {
Agent* current = m_agents[j];
double dist = pointDistance(current->x, item->x, current->y, item->y);
if (dist < closestDistance || closestDistance == -1) {
closestAgent = current;
closestDistance = dist;
}
}
item->group = closestAgent->cluster;
for (unsigned int j = 0; j < m_clusters.size(); j++) {
Cluster* cluster = m_clusters[j];
if (cluster->id == item->group)
cluster->points.push_back(item);
}
}
emit setClusters(&m_clusters);
}
void SprayTask::getSupplyAreaWithDistance(unsigned lastIndex, double maxWalk,
unsigned& chargeStart, unsigned& chargeEnd)
{
chargeStart = lastIndex;
chargeEnd = lastIndex;
for (int i = lastIndex + 1; i < (int)chargingLine.size(); i++)
{
if (pointDistance(chargingLine[lastIndex], chargingLine[i])>maxWalk)
{
break;
}
chargeEnd = i;
}
for (int i = lastIndex - 1; i > 0; i--)
{
if (pointDistance(chargingLine[lastIndex], chargingLine[i]) > maxWalk)
{
break;
}
chargeStart = i;
}
}
void SprayTask::findMiddlePointToGetChargingPath(DjiPoint pStart, DjiPoint pEnd, double distLeft,
int chargingStart, int chargingEnd, vector<DjiPoint>& goChargingPath, vector<DjiPoint>& continueTaskPath, unsigned& nearCharge)
{
if (pStart.s != 1)
printLog("input error, in findMiddlePointToGetChargingPath", true);
if (pStart.xyEqual(pEnd))
printLog("start and end point is same, infindMiddlePointToGetChargingPath", true);
pStart.s = 0;
pEnd.s = 0;
goChargingPath.clear();
continueTaskPath.clear();
vector<DjiPoint> pieceLine = disperseFlightLineseq(pStart, pEnd, pieceLength);
if (pieceLine.size() < 2)
{
printLog("pieceLine.size()<2, in findMiddlePointToGetChargingPath", true);
}
double comePointCost;
double gohomeCost;
int index = pieceLine.size() - 1;
for (index = pieceLine.size() - 1; index >= 0; index--)
{
comePointCost = pointDistance(pieceLine[0], pieceLine[index]);
gohomeCost = findNearestChargePoint(pieceLine[index], chargingStart, chargingEnd, nearCharge, goChargingPath);
if (distLeft > comePointCost + gohomeCost)
break;
}
if (goChargingPath.size() < 2)
{
printLog("askChargingError, in findMiddlePointToGetChargingPath", true);
}
for (int i = (int)goChargingPath.size() - 1; i >= 0; i--)
{
continueTaskPath.push_back(goChargingPath[i]);
}
continueTaskPath[continueTaskPath.size() - 1].s = 1;
continueTaskPath.push_back(pEnd);
if (index > 0)
{
vector<DjiPoint> task;
task.push_back(DjiPoint(pStart, 1));
task.insert(task.end(), goChargingPath.begin(), goChargingPath.end());
goChargingPath = task;
}
}
void CustomCircle::takePoint(Point p)
{
p2.x = p.x;
p2.y = p.y;
if (!On)
{
On = true;
p1.x = p.x;
p1.y = p.y;
}
clear_to_color(bmp, TRANSPARENT);
drawCircle(bmp, p1, pointDistance(p1, p2), GREEN);
}
vector<DjiPoint> disperseFlightLineseq(DjiPoint pStart, DjiPoint pEnd, double pieceLength)
{
if (pieceLength < 0 || (pStart.x == pEnd.x&&pStart.y == pEnd.y))
{
printLog("input error in dividLineSeq", true);
}
vector<DjiPoint> pieceLine;
double length = pointDistance(pStart, pEnd);
DjiVec normal = pEnd - pStart;
normal = normal / molMat(normal);
for (int j = 0; j*pieceLength < length; j++)
{
pieceLine.push_back(pStart + j*pieceLength*normal);
}
pieceLine.push_back(pEnd);
return pieceLine;
}
void deleteClosedPoint(vector<DjiPoint>& pts, double minDist)
{
if (pts.size() < 2)
return;
vector<DjiPoint> tem;
tem.push_back(pts[0]);
DjiPoint last = pts[0];
for (int i = 1; i < (int)pts.size(); i++)
{
if (pointDistance(pts[i], last)>minDist)
{
tem.push_back(pts[i]);
last = pts[i];
}
}
pts = tem;
}
Lines::Line::Line(CvPoint pt1, CvPoint pt2)
{
if (pt1.x == pt2.x)
{
Slope = ((float) (pt2.y - pt1.y)) / ((pt2.x+1) - pt1.x);
isVertical = true;
}
else
{
Slope = ((float) (pt2.y - pt1.y)) / (pt2.x - pt1.x);
isVertical = false;
}
Intercept = pt1.y - (Slope*pt1.x);
point1 = pt1;
point2 = pt2;
Length = pointDistance(point1, point2);
}
/* Calculates the total distance between two canvaes */
unsigned long int calculateDistance(CImg<unsigned char> * source, CImg<unsigned char> * drawing)
{
unsigned long int total_distance = 0;
for(int x = 0; x < source->width(); x++)
{
for(int y = 0; y < source->height(); y++)
{
char r = *(source->data(x,y,0,0));
char g = *(source->data(x,y,0,1));
char b = *(source->data(x,y,0,2));
char rr = *(drawing->data(x,y,0,0));
char gg = *(drawing->data(x,y,0,1));
char bb = *(drawing->data(x,y,0,2));
total_distance += pointDistance(rr, gg, bb, r, g, b);
}
}
return total_distance;
}
bool CustomCircle::makeBody(World *world)
{
b2Vec2 c1 = coordAllegToB2(p1);
b2Vec2 c2 = coordAllegToB2(p2);
float32 radius = cleanSize(pointDistance(p1, p2) / SCALE);
Body *b = world->makeCircle(c1, radius);
if (!b)
{
reset();
return false;
}
b->bmp = createCircleBitmap(radius, RANDOMCOLOR);
reset();
return true;
}
/* Calculates the total distance from the source image's inverse */
unsigned long int calculateWorstScore(CImg<unsigned char> * source, unsigned int bg_color)
{
unsigned long int total_distance = 0;
for(int x = 0; x < source->width(); x++)
{
for(int y = 0; y < source->height(); y++)
{
int r = *(source->data(x,y,0,0));
int g = *(source->data(x,y,0,1));
int b = *(source->data(x,y,0,2));
/* Pixel that is the furthest away from this pixel */
/* int r_inv = (r >= 128) ? 0 : 255; */
/* int g_inv = (g >= 128) ? 0 : 255; */
/* int b_inv = (b >= 128) ? 0 : 255; */
/* total_distance += pointDistance(r_inv, g_inv, b_inv, r, g, b); */
total_distance += pointDistance(bg_color, bg_color, bg_color, r, g, b);
}
}
return total_distance;
}
vector<DjiPoint> dispersePolyline(vector<DjiPoint>polyline, double pieceLength)
{
if (pieceLength < 0 || polyline.size() < 2)
{
printLog("input error in dividLineSeq", true);
}
vector<DjiPoint> pieceLine;
DjiVec normal;
double length = 0;
for (int i = 1; i < (int)polyline.size(); i++)
{
length = pointDistance(polyline[i - 1], polyline[i]);
normal = polyline[i] - polyline[i - 1];
normal = normal / molMat(normal);
for (int j = 0; j*pieceLength < length; j++)
{
pieceLine.push_back(polyline[i - 1] + j*pieceLength*normal);
}
}
pieceLine.push_back(polyline[polyline.size() - 1]);
return pieceLine;
}
void CLegGrid::filterLegs()
{
geometry_msgs::Pose new_leg, p1, p2;
/* check for close detected legs */
if(base_footprint_legs_.poses.size() < 2)
{
filtered_legs_.poses = base_footprint_legs_.poses;
}
else
{
if(!filtered_legs_.poses.empty()) filtered_legs_.poses.clear();
filtered_legs_.poses.push_back(base_footprint_legs_.poses.at(0));
for(uint8_t i = 1; i < base_footprint_legs_.poses.size(); i++)
{
p1.position = base_footprint_legs_.poses.at(i).position;
p2.position = filtered_legs_.poses.back().position;
if(sqrt(p1.position.x*p1.position.x + p1.position.y*p1.position.y) > 10.0) continue;
if(pointDistance(p1.position, p2.position) < 1.0)
{
filtered_legs_.poses.pop_back();
new_leg.position.x = (p1.position.x + p2.position.x) / 2;
new_leg.position.y = (p1.position.y + p2.position.y) / 2;
filtered_legs_.poses.push_back(new_leg);
} else
{
filtered_legs_.poses.push_back(p1);
}
}
}
}
void Robot::setGuardianLine(const QLineF &line,
const QPointF &footPoint)
{
//Clear the previous data.
resetGuardianLine();
//Check the length of the line.
if(line.length()==0.0)
{
return;
}
//Set has guardian line flag.
m_hasGuardianLine=true;
//Save the guardian line.
m_guardianLine=line;
//Get the opposite guardian line.
m_oppositeGuardianLine=QLineF(m_guardianLine.p2(), m_guardianLine.p1());
QLineF directionAngle=QLineF(QPointF(0.0, 0.0), QPointF(10.0, 0));
directionAngle.setAngle(m_angle);
//If the current angle is nearly to the angle, then the moving speed will be
//1.0(follow the direction of the line), or -1.0(reverse direction of the
//line)
if(m_oppositeGuardianLine.angleTo(directionAngle) <
m_guardianLine.angleTo(directionAngle))
{
m_movingSpeed=1.0;
m_angle=m_guardianLine.angle();
}
else
{
m_movingSpeed=-1.0;
m_angle=m_oppositeGuardianLine.angle();
}
//Move the robot to the foot point.
setPos(footPoint);
//Save the initial distance to p1.
m_toP1Distance=pointDistance(m_guardianLine.p1(), footPoint);
}
unsigned SprayTask::estimateNextChargingPoint(const vector<DjiPoint>& taskPoints, double reserve,
unsigned chargeStart, unsigned chargeEnd)
{
double _r = reserve;
double cost2next = 0;
unsigned nearCharge = 0;
double cost = 0;
double costSum = 0;
double used = 0;
int begainIndex = 0;
vector<DjiPoint> pathTem;
//飞机在i-1位置上
for (unsigned i = 1; i < taskPoints.size(); i++)
{
used += pointDistance(taskPoints[i - 1], taskPoints[i]);
if (used >= _r)
{
used -= pointDistance(taskPoints[i - 1], taskPoints[i]);
i = i - 1;
for (int j = i; j >= begainIndex; j--)
{
cost = findNearestChargePoint(taskPoints[j], chargeStart, chargeEnd, nearCharge, pathTem);
i = j;
double temp = used + cost;
if (used + cost < _r || j <= begainIndex)
{
if (j == begainIndex)
{
//如果最大电量不足以飞行完一个航点 返回错误
if (j + 1 < (int)taskPoints.size())
{
if (maxFlightDist < cost + pointDistance(taskPoints[j], taskPoints[j + 1]) +
findNearestChargePoint(taskPoints[j + 1], chargeStart, chargeEnd, nearCharge, pathTem))
printLog("最大电量不足以飞完一个航点, in batteryCheck", true);
}
}
//taskPoints[j].s = P_CHARGE;//换电池标记
costSum += cost;//飞机换完电池返回上次的任务点
_r = maxFlightDist - cost;
getSupplyAreaWithDistance(nearCharge, maxGroundWalk, chargeStart, chargeEnd);
begainIndex = j;
}
else
{
used -= pointDistance(taskPoints[j - 1], taskPoints[j]);
}
}
used = 0;
//once
return nearCharge;
}
}
reserve = _r - used;
findNearestChargePoint(taskPoints[taskPoints.size() - 1], chargeStart, chargeEnd, nearCharge, pathTem);
return nearCharge;
}
/*!
Distance between the line and a specified point
\param thePoint
Point to use for distance computation
Punto di cui calcolare la distanza dalla linea
\return
The distance of thePoint from this, or -DBL_MAX if this or thePoint are not valid
*/
double GM_3dLine::pointDistance(GM_3dPoint thePoint) const {
GM_3dPoint dummyPoint;
return pointDistance(thePoint, dummyPoint);
}
bool SprayTask::askCharging(int waypointIndex, double powerPercent)
{
chargingPath = ChargingPath();
if (powerPercent > 100 || powerPercent < 0)
{
printLog("powerPercent out of range !", true);
return false;
}
if (waypointIndex >= (int)waypoints.size() || waypointIndex < 0)
{
printLog("waypointIndex out of range !", true);
return false;
}
// if (powerPercent < safeLandPercent)
// {
// printLog("powerPercent < safeLandPercent!!!!!", true);
// }
vector<DjiPoint> goChargingPath;
vector<DjiPoint> continueTaskPath;
int usedIndex = -1; //使用了的航电index continuepath还要保证该index后面到index+1的航线段都飞行完毕
unsigned nearCharge = 0;
if (waypointIndex == waypoints.size() - 1)
{
printLog("the last waypoint, task complete !!!!", false);
findNearestChargePoint(waypoints[waypointIndex], supplyStart, supplyEnd, nearCharge, goChargingPath);
if (isUseCoordTransform)
goChargingPath = tran.plane2latlong(goChargingPath);
//最后一个航点
chargingPath = ChargingPath(goChargingPath);
return true;
}
//计算当前power还能飞多远
double distLeft = maxFlightDist*(powerPercent - minPowerPercent) / (100.0 - minPowerPercent);
//是否能够飞完下一个waypoint + gohome 的电量
double gohomeCost = findNearestChargePoint(waypoints[waypointIndex + 1], supplyStart, supplyEnd, nearCharge, goChargingPath);
double goNextCost = pointDistance(waypoints[waypointIndex], waypoints[waypointIndex + 1]);
//无需更换电池
if (gohomeCost + goNextCost < distLeft)
return false;
else//gohomeCost + goNextCost > distLeft
{
//********************* Print Log ***********************//
printLog("minPowerPercent=" + double2string(minPowerPercent));
printLog("waypointIndex=" + int2string(waypointIndex));
printLog("powerPercent=" + double2string(powerPercent));
printLog("maxFlightDist=" + double2string(maxFlightDist));
printLog("distLeft =" + double2string(distLeft));
printLog("goNextCost=" + double2string(goNextCost));
printLog("gohomeCost= " + double2string(gohomeCost));
printLog("goNextCost+gohomeCost =" + double2string(goNextCost + gohomeCost) + " > distLeft:" + double2string(distLeft));
if (!(waypoints[waypointIndex].s == 0 || waypoints[waypointIndex].s == 1))
{
printLog("waypoints[waypointIndex] with erro s=" + int2string(waypoints[waypointIndex].s), true);
}
//********************* Print Log End ***********************//
//下一段航线不用喷洒, 从当前waypointIndex返航换电池
if (waypoints[waypointIndex].s == 0)
{
//直接从当前点回家
findNearestChargePoint(waypoints[waypointIndex], supplyStart, supplyEnd, nearCharge, goChargingPath);
for (usedIndex = waypointIndex + 1; usedIndex < (int)waypoints.size(); usedIndex++)
{
//断电续航时飞到下一个开始喷洒的位置
if (waypoints[usedIndex].s == 1)
{
findTwoPointsPath(allAroundObsEdgesObj, chargingLine[nearCharge], waypoints[usedIndex], continueTaskPath);
break;
}
}
//任务结束，无断电续航的的continueTaskPath
if (continueTaskPath.size() == 0)
{
chargingPath = ChargingPath(isUseCoordTransform ? tran.plane2latlong(goChargingPath) : goChargingPath);
return true;
}
//保证usedIndex 到usedIndex之间的航线段飞完
if (usedIndex + 1 < (int)waypoints.size())
{
continueTaskPath.push_back(waypoints[usedIndex + 1]);
}
}
//下一段为喷洒航线段，从当前waypointIndex继续喷洒，直到剩余电量刚好够回家
else if (waypoints[waypointIndex].s == 1)
{
findMiddlePointToGetChargingPath(waypoints[waypointIndex], waypoints[waypointIndex + 1],
distLeft, supplyStart, supplyEnd, goChargingPath, continueTaskPath, nearCharge);
usedIndex = waypointIndex;
}
continueTaskPath.insert(continueTaskPath.end(), waypoints.begin() + usedIndex + 1, waypoints.end());
getSupplyAreaWithDistance(nearCharge, maxGroundWalk, supplyStart, supplyEnd);
unsigned estimateNext = estimateNextChargingPoint(continueTaskPath, maxFlightDist, supplyStart, supplyEnd);
getSupplyAreaWithDistance(estimateNext, landingDeviation, supplyStart, supplyEnd);
DjiPoint nextChargingPoint = chargingLine[estimateNext];
vector<DjiPoint> supplyArea = getRange(chargingLine, supplyStart, supplyEnd);
//更新waypoints
waypoints = continueTaskPath;
if (isUseCoordTransform)
{
goChargingPath = tran.plane2latlong(goChargingPath);
continueTaskPath = tran.plane2latlong(continueTaskPath);
nextChargingPoint = tran.plane2latlong(nextChargingPoint);
supplyArea = tran.plane2latlong(supplyArea);
}
chargingPath = ChargingPath(goChargingPath, continueTaskPath, supplyArea);
return true;
}
return false;
}
double SprayTask::batteryCheck(const vector<DjiPoint>& taskPoints, double& reserve,
unsigned& chargeStart, unsigned& chargeEnd, int& changeBatteryTimes)
{
double _r = reserve;
double cost2next = 0;
unsigned nearCharge;
double cost = 0;
double costSum = 0;
double used = 0;
int begainIndex = 0;
vector<DjiPoint> pathTem;
changeBatteryTimes = 0;
//飞机在i-1位置上
for (unsigned i = 1; i < taskPoints.size(); i++)
{
used += pointDistance(taskPoints[i - 1], taskPoints[i]);
if (used >= _r)
{
used -= pointDistance(taskPoints[i - 1], taskPoints[i]);
i = i - 1;
for (int j = i; j >= begainIndex; j--)
{
cost = findNearestChargePoint(taskPoints[j], chargeStart, chargeEnd, nearCharge, pathTem);
i = j;
double temp = used + cost;
if (used + cost < _r || j <= begainIndex)
{
if (j == begainIndex)
{
//如果最大电量不足以飞行完一个航点 返回错误
if (j + 1 < (int)taskPoints.size())
{
if (maxFlightDist < cost + pointDistance(taskPoints[j], taskPoints[j + 1]) +
findNearestChargePoint(taskPoints[j + 1], chargeStart, chargeEnd, nearCharge, pathTem))
printLog("最大电量不足以飞完一个航点, in batteryCheck", true);
}
}
//taskPoints[j].s = P_CHARGE;//换电池标记
costSum += cost;//飞机换完电池返回上次的任务点
_r = maxFlightDist - cost;
getSupplyAreaWithDistance(nearCharge, maxGroundWalk, chargeStart, chargeEnd);
begainIndex = j;
changeBatteryTimes++;
//**//
vector<DjiPoint> pts;
pts.push_back(taskPoints[i - 1]);
pts.push_back(chargingLine[nearCharge]);
//drawImg(mapImg, pts, CV_RGB(255, 0, 0), CV_DRAW_POINTS);
break;
}
else
{
used -= pointDistance(taskPoints[j - 1], taskPoints[j]);
}
}
used = 0;
}
}
reserve = _r - used;
return costSum;
}
void AI::generateMoves(fizGTableState state, set<shot>& moves, turn_T turn){
//for each ball
fizBall cueBall = state.getBall(CUE);
//must have been a scratch, ball in hand...figure out what to do
if(!inPlay(cueBall)) return;
fizPoint cueBallPos = cueBall.getPos();
BallType firstBall, lastBall;
switch(turn){
case stripes:
firstBall = EIGHT;
lastBall = FIFTEEN;
break;
case solids:
firstBall = ONE;
lastBall = EIGHT;
break;
case table_open:
firstBall = ONE;
lastBall = FIFTEEN;
break;
}
for(BallType ballNum = firstBall; ballNum <= lastBall; ballNum = (BallType)(ballNum + 1)){
if(ballNum == EIGHT && !isOnly8Left(state, (turn == solids))) continue;
fizBall ball = state.getBall(ballNum);
//if ball is not in play
if(!inPlay(ball)) continue;
fizPoint ballPos = ball.getPos();
//for each pocket, generate a straight in shot
int numOfPockets = 6;
BoundaryId pockets[] = {SW_POCKET, W_POCKET, NW_POCKET, NE_POCKET, E_POCKET, SE_POCKET};
for(int i = 0; i < numOfPockets; i++){
//get ball position, cue ball position, and pocket position
//calculate ghost ball position
//calculate theta, a, b, phi, and minVelocity
fizPoint pocketPos = theTable->getPocketCenter(pockets[i]);
//make sure shot is possible
//if(!shotPossible(cueBallPos, ballPos, pocketPos)) continue;
//get ball-pocket line and cue-ghostball line
double yDiff = fabs(pocketPos.y - ballPos.y);
double xDiff = fabs(pocketPos.x - ballPos.x);
double ballPocketAngle = atan( yDiff / xDiff);
double deltaX = ballDiameter * cos(ballPocketAngle);
double deltaY = ballDiameter * sin(ballPocketAngle);
if(ballPos.x < pocketPos.x) deltaX = -deltaX;
if(ballPos.y < pocketPos.y) deltaY = -deltaY;
double ghostX = ballPos.x + deltaX;
double ghostY = ballPos.y + deltaY;
fizPoint ghostPos(ballPos.x + deltaX, ballPos.y + deltaY);
//make sure no balls are in the way
//this should also make sure that the shot is possible because
//if it is not possible then the target ball will be in the way of the cue and ghost
vec2 p1;
p1.x = cueBallPos.x;
p1.y = cueBallPos.y;
vec2 p2;
p2.x = ghostPos.x;
p2.y = ghostPos.y;
if(!isTrajectoryClear(p1, p2, state, -1)) continue;
p1.x = ballPos.x;
p1.y = ballPos.y;
p2.x = pocketPos.x;
p2.y = pocketPos.y;
if(!isTrajectoryClear(p1, p2, state, -1)) continue;
//ok create the shot
//set a, b to zero
//find minimum velocity
//find cue angle and elevation angle
shot theShot;
theShot.a = 0; //positive is to right of center (mm)
theShot.b = 0; //positive is up from center (mm)
//theShot.v = 1; //have to calculate the minimum required velocity (m/s) max = 4.5
xDiff = fabs(cueBallPos.x - ghostPos.x);
yDiff = fabs(cueBallPos.y - ghostPos.y);
theShot.phi = atan(yDiff / xDiff);
//convert to degrees
theShot.phi *= (180.0 / M_PI);
//find true angle
if(cueBallPos.x < ghostPos.x){
if(cueBallPos.y > ghostPos.y){
theShot.phi = -theShot.phi;
}
else{
//have correct angle
}
}
else{
if(cueBallPos.y < ghostPos.y){
theShot.phi = 180 - theShot.phi;//90 + theShot.phi;
}
else{
theShot.phi = 180 + theShot.phi; //180 + theShot.phi;
}
}
//distance from ball to pocket
theShot.d1 = pointDistance(ballPos, pocketPos);
//distance from cue to ghost
theShot.d2 = pointDistance(cueBallPos, ghostPos);
//slopes of lines, to determine angles
yDiff = ballPos.y - pocketPos.y;
xDiff = ballPos.x - pocketPos.x;
double yDiff2 = cueBallPos.y - ghostPos.y;
double xDiff2 = cueBallPos.x - ghostPos.x;
theShot.cutAngle = angleBetweenLines(yDiff, xDiff, yDiff2, xDiff2);
//determine the pocket angle
bool isCorner = true;
switch(pockets[i]){
case W_POCKET:
case E_POCKET:
yDiff2 = 0;
xDiff2 = 1;
isCorner = false;
break;
case SW_POCKET:
yDiff2 = 1;
xDiff2 = 1;
break;
case NW_POCKET:
yDiff2 = -1;
xDiff2 = 1;
break;
case NE_POCKET:
yDiff2 = -1;
xDiff2 = -1;
break;
case SE_POCKET:
yDiff2 = 1;
xDiff2 = -1;
break;
}
theShot.pocketAngle = angleBetweenLines(yDiff, xDiff, yDiff2, xDiff2);
theShot.v = vTable.getMinVelocity(theShot.d2, theShot.d1, theShot.cutAngle) + .2;
if(theShot.v < 0){
continue;
}
ShotData shotCopy;
shotCopy.a = theShot.a;
shotCopy.b = theShot.b;
shotCopy.v = theShot.v;
shotCopy.theta = 2;
shotCopy.phi = theShot.phi;
theShot.theta = findMinTheta(*theTable, state, shotCopy); //elevation angle (degrees)
theShot.ball = ballNum;
theShot.pocket = pockets[i];
theShot.probability = pTable.getProb(theShot.d1, theShot.d2, theShot.cutAngle, theShot.pocketAngle, isCorner);
if(theShot.probability == 0) theShot.probability = .001;
//add shot to the list
moves.insert(theShot);
}
}
}
bool isSamePoint(Point & a, Point & b) {
return pointDistance(a, b) < MIN_NODE_DISTANCE;
}
int main(int argc, char const *argv[])
{
char* s;
std::srand(std::time(0)); //use current time as seed for random generator
int r = rand() % 1000;
for(int i = 0; i < r; i++)
{
rand();
}
if(argc < 3)
{
return 1;
}
int forestSize = strtol(argv[1], &s, 10);
int iterations = strtol(argv[2], &s, 10);
double SIDE = std::sqrt(forestSize);
SIDE = fRand(std::sqrt(SIDE),std::sqrt(2)*SIDE);
double R = 1;
double begin, end;
std::vector<int> empty;
std::vector<Tree*> Forest;
std::vector< std::vector<int> > neighbors(forestSize,empty);
std::vector<double> metrics(forestSize,0.0);
int num_threads;
std::vector<int> systems_processed; // DEBUG
std::vector<int> symbols_translated; // DEBUG
///// PARALLEL BLOCK
begin = omp_get_wtime();
#pragma omp parallel shared(Forest,neighbors,metrics,num_threads)
{
#pragma omp master
{
// INIT VARIABLES
std::vector<Point> positions;
num_threads = omp_get_num_threads();
std::cout << "Running " << forestSize << " trees for " << iterations << " iterations on " << num_threads << " processors" << std::endl;
for(int i = 0; i < forestSize; i++)
{
double x = fRand(0,SIDE);
double y = fRand(0,SIDE);
Point p = {x,y};
Tree *T = new MonopodialTree();
Forest.push_back(T);
positions.push_back(p);
for(int j = 0 ; j < i ; j++)
{
Point q = positions[j];
if(pointDistance(p,q) < R)
{
neighbors[j].push_back(i);
neighbors[i].push_back(j);
}
}
}
systems_processed = std::vector<int>(num_threads,0); //DEBUG
symbols_translated = std::vector<int>(num_threads,0); //DEBUG
}
#pragma omp barrier
int thread_num = omp_get_thread_num();
// ITERATE
for(int j = 0 ; j < iterations ; j++)
{
#pragma omp for schedule(dynamic)
for(int i = 0; i < Forest.size() ; i++)
{
Forest[i]->next();
double metric = Forest[i]->calculateMetric();
metrics[i] = metric;
systems_processed[thread_num]++; //DEBUG
symbols_translated[thread_num] += Forest[i]->getState().size(); //DEBUG
}
#pragma omp for schedule(dynamic)
for(int i = 0; i < Forest.size() ; i++)
{
Forest[i]->updateMetric(metrics,neighbors[i]);
}
}
}
///// PARALLEL BLOCK
end = omp_get_wtime();
std::vector< std::vector<int> > connected_components = get_connected_components(neighbors);
// print_forest(Forest, neighbors, metrics); // VERBOSE
// print_connected_components( connected_components); // VERBOSE
char buffer[80];
FILE *f = fopen("Results_naive.txt", "a");
if(f != NULL)
{
fprintf(f, "%s\n", gettime(buffer));
fprintf(f,"%d threads\n",num_threads);
fprintf(f,"%d trees\n",forestSize);
fprintf(f,"%d iterations\n",iterations);
fprintf(f,"%lf %lf\n",SIDE,R);
for(int i = 0; i < connected_components.size(); i++)
{
fprintf(f, "%d ", connected_components[i].size());
}
fprintf(f, "\n");
fprintf(f,"Proc Systems Symbols\n");//DEBUG
for(int i = 0; i < num_threads; i++)//DEBUG
{//DEBUG
fprintf(f," %02d %03d %03d\n",i,systems_processed[i],symbols_translated[i]);//DEBUG
}//DEBUG
fprintf(f,"Time : %f seconds\n", end-begin);
fprintf(f,"\n=====================\n");
}
for(int i = 0; i < Forest.size() ; i++)
{
delete Forest[i];
}
return 0;
}