/*
* Manhattan distance
*/
float AStarPlanner::manhattan_distance(Util::Point start, Util::Point end) {
Util::Point x1 = Util::Point(start.x, 0.0f, 0.0f);
Util::Point x2 = Util::Point(end.x, 0.0f, 0.0f);
Util::Point z1 = Util::Point(0.0f, 0.0f, start.z);
Util::Point z2 = Util::Point(0.0f, 0.0f, end.z);
float man_dist = distanceBetween(x1, x2) + distanceBetween(z1, z2);
return man_dist;
}
void Tank::runAI(){
float dist = distanceBetween(this->givePosX(), this->givePosZ(), playerTank->givePosX(), playerTank->givePosZ());
this->sightCounter -= 1;
if (dist < this->sightRange){
this->sightCounter = 10;
this->canSeePlayer = true;
}
else {
if (this->sightCounter < 0) {
this->canSeePlayer = false;
this->destinX = this->lastSightingX;
this->destinZ = this->lastSightingZ;
}
}
if(this->canSeePlayer){
float bulletCyclesToTarget = dist/bulletSpeed;
float pX = playerTank->givePosX() - bulletCyclesToTarget*playerTank->speed*sin(playerTank->rotation*PI/180) - bulletCyclesToTarget*this->speedX;
float pZ = playerTank->givePosZ() - bulletCyclesToTarget*playerTank->speed*cos(playerTank->rotation*PI/180) - bulletCyclesToTarget*this->speedZ;
this->turnTurretToward(this->angleTo(pX, pZ));
if (this->isAimed){
this->fire();
}
this->lastSightingX = playerTank->givePosX();
this->lastSightingZ = playerTank->givePosZ();
}
else {
}
if (distanceBetween(this->posX, this->posZ, this->destinX, this->destinZ) > 5.0f) {
this->atDestination = false;
}
else {
this->atDestination = true;
}
if (!(rand() % 30) && this->canSeePlayer){
this->destinX += rand() % 20 - 10;
this->destinZ += rand() % 20 - 10;
}
if (this->atDestination){
this->rotate(true);
this->accelerate(true);
}
else {
this->turnToward(this->angleTo(this->destinX, this->destinZ));
this->accelerate(true);
}
}
void sortByDistance(vector<ofColor> &colors) {
if (colors.size() == 0) {
return;
}
int totalCount = colors.size();
ofColor root = getDarkest(colors);
// put it in the sorted list as starting element.
vector<ofColor> sorted;
sorted.push_back(root);
// Remove the darkest color from the stack,
vector<ofColor> stack(colors);
vector<ofColor>::iterator it = std::find(stack.begin(), stack.end(), root);
// ofLog()<<"erase darkest "<<(*it)<<endl;
//if(it != stack.end()){
stack.erase(it);
// }
// Now find the color in the stack closest to that color.
// Take this color from the stack and add it to the sorted list.
// Now find the color closest to that color, etc.
int sortedCount = 0;
while (stack.size() > 1) {
ofColor closest = stack[0];
ofColor lastSorted = sorted[sortedCount];
float distance = distanceBetween(closest, lastSorted);
// ofLog()<<"searching: "<<endl;
int foundId=0;
for (int i = stack.size() - 1; i >= 0; i--) {
ofColor c = stack[i];
float d = distanceBetween(c, lastSorted);
// ofLog()<<"dist: "<<d<< " stack.size() "<<stack.size()<< " sortedCount "<<sortedCount<<" totalCount "<<totalCount <<endl;
if (d < distance) {
closest = c;
distance = d;
foundId=i;
}
}
sorted.push_back(closest);
stack.erase(stack.begin()+foundId);
sortedCount++;
}
sorted.push_back(stack[0]);
colors = sorted;
}
// make sure to check enemies size > 0 (more than 0 enemies) before calling this function or it will cause exception
Enemy* Bakoom::nearestEnemyTo(int x, int y)
{
int shortestDistance = distanceBetween(x, y, enemies_.at(0)->getRect().x(), enemies_.at(0)->getRect().y());
int nearestEnemyIndex = 0;
for(int i = 0; i < enemies_.size(); i++)
{
int currentEnemyDistance = distanceBetween(x, y, enemies_.at(i)->getRect().x(), enemies_.at(i)->getRect().y());
if(currentEnemyDistance < shortestDistance)
{
shortestDistance = currentEnemyDistance;
nearestEnemyIndex = i;
}
}
return enemies_.at(nearestEnemyIndex);
}
void Ball::mergeBalls(Ball* ball1, Ball* ball2, double delta) {
if(ball1->isMarkedForDeletion || ball2->isMarkedForDeletion) return;
if(!((distanceBetween(ball1, ball2) < ball1->radius + ball2->radius))) return;
Ball* big;
Ball* small;
if(ball1->getMass() >= ball2->getMass()) {
big = ball1;
small = ball2;
} else {
big = ball2;
small = ball1;
}
double massCenterX = (big->getMass() * big->getX() + small->getMass() * small->getX()) / (big->getMass() + small->getMass());
double massCenterY = (big->getMass() * big->getY() + small->getMass() * small->getY()) / (big->getMass() + small->getMass());
big->setX(massCenterX);
big->setY(massCenterY);
big->setVelX((big->getMass() * big->getVelX() + small->getMass() * small->getVelX()) / (big->getMass() + small->getMass()));
big->setVelY((big->getMass() * big->getVelY() + small->getMass() * small->getVelY()) / (big->getMass() + small->getMass()));
big->color = {
(unsigned char)((big->getMass() * big->color.r + small->getMass() * small->color.r) / (big->getMass() + small->getMass())),
(unsigned char)((big->getMass() * big->color.g + small->getMass() * small->color.g) / (big->getMass() + small->getMass())),
(unsigned char)((big->getMass() * big->color.b + small->getMass() * small->color.b) / (big->getMass() + small->getMass()))
};
big->setMass(big->getMass() + small->getMass());
big->recalculateRadius();
small->isMarkedForDeletion = true;
}
void Camera::Transform(const Matrix4x4& trans, bool maintainDistance)
{
Matrix4x4 frame = InitWithBasis();
Matrix4x4 frame_inv = inverse(frame);
Matrix4x4 final_trans = frame * trans * frame_inv;
ElVisFloat4 up4 = MakeFloat4(m_v);
ElVisFloat4 eye4 = MakeFloat4(m_eye);
eye4.w = 1.0f;
ElVisFloat4 lookat4 = MakeFloat4(m_lookAt);
lookat4.w = 1.0f;
// Floating point errors tend to cause the camera to move away or towards
// the
// look at point. Store the distance so we can restore it after the
// transform.
ElVisFloat distance = distanceBetween(m_lookAt, m_eye);
WorldVector temp_up = WorldVector(final_trans * up4);
WorldPoint temp_eye = WorldPoint(final_trans * eye4);
WorldPoint temp_lookAt = WorldPoint(final_trans * lookat4);
if (maintainDistance)
{
WorldVector restoreVector = createVectorFromPoints(temp_lookAt, temp_eye);
restoreVector.Normalize();
temp_eye = findPointAlongVector(restoreVector, distance) + temp_lookAt;
}
WorldVector reducedUp(temp_up.x(), temp_up.y(), temp_up.z());
SetParameters(temp_eye, temp_lookAt, reducedUp);
}
void Camera::Pan(
int from_x, int from_y, int to_x, int to_y, int imageWidth, int imageHeight)
{
double percentVertical =
static_cast<double>(to_y - from_y) / static_cast<double>(imageHeight);
double percentHorizontal =
static_cast<double>(-to_x + from_x) / static_cast<double>(imageWidth);
double viewportSpan_modelCoords;
if( m_perspectiveMode )
{
ElVisFloat cameraDistance = distanceBetween(m_eye, m_lookAt);
viewportSpan_modelCoords = 2 * tan (m_fieldOfView / 2 * 3.141593 / 180.0) * cameraDistance;
} else {
viewportSpan_modelCoords = 2 * m_orthoBorder;
}
WorldVector cameraV = m_v;
Normalize(cameraV);
WorldVector cameraU = m_u;
Normalize(cameraU);
WorldVector dir = percentVertical * viewportSpan_modelCoords * cameraV
+ percentHorizontal * viewportSpan_modelCoords * cameraU;
WorldPoint pointIncr = findPointAlongVector(dir, 1.0);
m_eye = m_eye + pointIncr;
m_lookAt = m_lookAt + pointIncr;
OnCameraChanged();
}
double PairRelationDetector::ConnectedPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0),mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Structure::Link* link = RelationDetector::findLink(n1, n2, this->graph_);
if ( link == 0)
return deviation;
if ( this->bUseLink_)
deviation = RelationDetector::computeDeviationByLink(link);
else
{
distanceBetween(m1, m2, deviation, mean_dist, max_dist);
}
deviation = deviation/this->normalizeCoef_;
if ( deviation > this->maxAllowDeviation_)
{
this->maxAllowDeviation_ = deviation;
}
return deviation;
}
void SimObject::calculateSprings(SimObject* anotherObject, double delta,
double springMaxDistance, double springDistance, double springDamping, double springForce) {
double distance = distanceBetween(this, anotherObject);
if(distance == 0) return;
if(springMaxDistance > 0 && distance > springMaxDistance) {
springConnections.erase(std::remove(springConnections.begin(), springConnections.end(), anotherObject), springConnections.end());
anotherObject->incomingSpringConnectionsCount--;
return;
}
double offset = distance - springDistance;
double relativeSpeedX = anotherObject->getVelX() - getVelX();
double relativeSpeedY = anotherObject->getVelY() - getVelY();
//TODO remake damping
double relativeSpeed = sqrt(relativeSpeedX*relativeSpeedX + relativeSpeedY*relativeSpeedY);
double dampingForce = relativeSpeed * springDamping;
double dampingForceX, dampingForceY;
if(relativeSpeed != 0) {
dampingForceX = relativeSpeedX / relativeSpeed * dampingForce;
dampingForceY = relativeSpeedY / relativeSpeed * dampingForce;
} else {
dampingForceX = 0;
dampingForceY = 0;
}
double force;
force = offset * springForce - dampingForce;
double forceX = (anotherObject->getX() - getX()) / distance * force + dampingForceX;
double forceY = (anotherObject->getY() - getY()) / distance * force + dampingForceY;
double velX = getVelX();
double velY = getVelY();
velX += forceX / getMass() * delta;
velY += forceY / getMass() * delta;
setVelX(velX);
setVelY(velY);
}
bool PairRelationDetector::has_trans_relation(int id1, int id2)
{
Eigen::Vector3d transVec = nodesCenter_[id1] - nodesCenter_[id2];
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(nodesCpts_[id2], transMat);
double min_dist, mean_dist, max_dist;
distanceBetween(nodesCpts_[id1], newverts2, min_dist, mean_dist, max_dist);
double error = 2 * mean_dist / (nodesDiameter_[id1] + nodesDiameter_[id2]);
double nerror = fixDeviationByPartName(graph_->nodes[id1]->id, graph_->nodes[id2]->id, error);
if (nerror < thTransRadio_)
{
PairRelation pr(graph_->nodes[id1], graph_->nodes[id2]);
pr.trans_vec = transVec;
pr.diameter = computePairDiameter(pr);
pr.deviation = error;
pr.tag = true;
transPairs_.push_back(pr);
return true;
}
else
return false;
}
int distanceBetweenAll(void){
int sum = 0, i;
for (i = 0; i < num; i++) {
sum += distanceBetween(me, points[i]);
}
return sum;
}
double PairRelationDetector::RefPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0), min_dist, mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Eigen::Vector3d refCenter = (n1->center() + n2->center())*0.5;
Eigen::Vector3d refNormal = n1->center() - n2->center();
refNormal.normalize();
Eigen::MatrixXd newverts2;
reflect_points3d(m2, refCenter, refNormal, newverts2);
distanceBetween(m1, newverts2, min_dist, mean_dist, max_dist);
deviation = 2 * mean_dist / (n1->bbox().diagonal().norm() + n2->bbox().diagonal().norm());
if ( deviation > maxAllowDeviation_)
{
maxAllowDeviation_ = deviation;
}
return deviation;
}
chemkit::Variant GraphRadiusDescriptor::value(const chemkit::Molecule *molecule) const
{
int radius = std::numeric_limits<int>::max();
for(size_t i = 0; i < molecule->size(); i++){
const chemkit::Atom *a = molecule->atom(i);
int eccentricity = 0;
for(size_t j = 0; j < molecule->size(); j++){
const chemkit::Atom *b = molecule->atom(j);
int distance = distanceBetween(a, b);
if(distance > eccentricity){
eccentricity = distance;
}
}
if(eccentricity < radius){
radius = eccentricity;
}
}
return radius;
}
void Bakoom::timerEvent(QTimerEvent *event)
{
// create random scenery
createRandomScenery();
// level 6 and 7 enemy spawns
if(level >= 6)
for(int i = 0; i < enemies_.size(); i++)
if(rand()%1000 < 5 && enemies_.size() <= 12) enemies_.push_back(new Interceptor(enemies_.at(i)->getRect().x(), enemies_.at(i)->getRect().y()));
// move scenery
for(int i = 0; i < scenery_.size(); i++)
scenery_.at(i)->autoMove();
// move enemies
moveEnemies();
// make enemies shoot
createEnemyProjectiles();
// automove credits
for(int i = 0; i < credits_.size(); i++)
credits_.at(i)->autoMove();
// automove medipaks_
for(int i = 0; i < medipaks_.size(); i++)
medipaks_.at(i)->autoMove();
// automove projectiles_
for(int i = 0; i < projectiles_.size(); i++)
{
// homing projectiles_
if(projectiles_.at(i)->isHoming())
{
int proj_x = projectiles_.at(i)->getRect().x(), proj_y = projectiles_.at(i)->getRect().y();
int enem_x = nearestEnemyTo(proj_x, proj_y)->getRect().x(), enem_y = nearestEnemyTo(proj_x, proj_y)->getRect().y();
if(distanceBetween(enem_x, enem_y, proj_x, proj_y) < 250)
{
if(projectiles_.at(i)->isEnemyProjectile())
enem_x = player_ ->getRect().x();
if(enem_x+40 > proj_x) projectiles_.at(i)->setXdir(1);
else if(enem_x-40 < proj_x) projectiles_.at(i)->setXdir(-1);
else projectiles_.at(i)->setXdir(0);
}
}
projectiles_.at(i)->autoMove();
}
checkCollision();
checkOutOfBounds();
deleteDead();
// check for win condition
if(enemies_.size() == 0 && credits_.size() == 0)
{
shield = player_ ->getHealth(); // update shield variable before exiting
qApp->exit();
}
// auto heal if applicable
if(hasItem[15])
autoHeal();
playerKeyEvents();
//deleteDead();
// move player
player_->autoUpdateRect();
spawnMedipaks();
repaint();
}
int Cross::checkPreconditions(SquadPosition* player, Stadium* stadium)
{
// If player is far from ball, he cannot shoot.
if ( distanceBetween(player->getPosition(),stadium->ball->getPosition2()) > REACHING_DISTANCE )
return 0;
return 0;
};
void Camera::SetupOpenGLOrtho()
{
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
m_orthoBorder = distanceBetween(m_eye, m_lookAt) / EYE_INIT_Z * 3;
glOrtho(-m_orthoBorder, m_orthoBorder, -m_orthoBorder, m_orthoBorder, -1000.0, 1000.0);
glMatrixMode(GL_MODELVIEW);
m_perspectiveMode = false;
}
bool Tank::canMoveTo(float newX, float newZ){
if (newX > mapSize - 1.0f || newX < -mapSize + 1.0f || newZ > mapSize - 1.0f || newZ < -mapSize + 1.0f){
return false;
}
for (uint i = 0; i < tanks.size(); i++) {
if (this != tanks[i] && distanceBetween(newX, newZ, tanks[i]->posX, tanks[i]->posZ) < 1.5f){
return false;
}
}
for (uint i = 0; i < obstacles.size(); i++) {
if (distanceBetween(newX, newZ, obstacles[i]->givePosX(), obstacles[i]->givePosZ()) < obstacles[i]->giveRadius() + 0.5f){
return false;
}
}
if (this != playerTank && distanceBetween(newX, newZ, playerTank->posX, playerTank->posZ) < 1.5f) {
return false;
}
return true;
}
int main(int argc, char* argv[]){
std::vector< std::pair<int, CoordPair> > original;
std::string inputFile(argv[1]);
buildCoordPairSet(inputFile, original);
std::vector< std::pair<int, CoordPair> > pairs = original;
srand(time(NULL));
time_t curTime = time(NULL);
time_t stop = time(NULL) + (TIME_LIMIT);
int count = 0;
while(curTime < stop && count < 100){
curTime = time(NULL);
int i = rand() % (pairs.size()-1); //Range: 0 - (size-2)
int j = rand() % pairs.size(); //Range: 0 - (size-1)
if(i == j) continue; //If indices are equal
if(distanceBetween(pairs[i].second, pairs[j].second) < distanceBetween(pairs[i].second, pairs[i+1].second)){
swapVecInd(pairs, i+1, j);
count = 0;
} else {
count++;
}
}
int dist;
for(int i = 1; i < pairs.size(); i++){
dist += distanceBetween(pairs[i].second, pairs[i-1].second);
}
dist += distanceBetween(pairs[pairs.size()-1].second, pairs[0].second);
std::cout << "Total Distance: " << dist << std::endl;
std::string outputFile("sol.tsp3." + inputFile.substr(0, 3)+ inputFile.at(inputFile.size()-5) + ".txt");
std::ofstream sol(outputFile.c_str(), std::ofstream::out);
sol << dist << std::endl;
for(int i = 0; i < pairs.size(); i++){
sol << pairs[i].first << std::endl;
}
sol.close();
return 0;
}
//checks if any bullets have collided with any missiles
void checkCollisions(){
for(int i = 0;i<MAX_BULLETS;i++){
for(unsigned int j = 0; j<missiles.size();j++){
if(!missiles[j].destroyed)
if(distanceBetween(i,j) < (BULLET_SIZE*2.5)){
missiles[j].destroyed=true;
numDestroyed++;
}
}
}
}
// Helper method which attempts to add a SearchNode to the output vector if it is traversable.
void AStarPlanner::_tryToAdd(unsigned int x, unsigned int z, const SearchNodePtr& from, float cost, Util::Point goal, std::vector<SearchNodePtr>& out) {
int index = gSpatialDatabase->getCellIndexFromGridCoords(x, z);
if (!canBeTraversed(index)) return;
Util::Point p;
gSpatialDatabase->getLocationFromIndex(index, p);
// CHANGE weight for the weighted A*
float weight = 1;
// CHANGE to distanceBetween for Euclidean distance and manhattan_distance for Manhattan distance
float h = distanceBetween(p, goal) * weight;
out.push_back(SearchNodePtr(new SearchNode(index, from->g() + cost, h)));
}
void SimObject::calculateGravity(SimObject* anotherObject, double delta, double gravityRadialForce) {
double distance = distanceBetween(this, anotherObject);
if(distance == 0) return;
double force = gravityRadialForce * getMass() * anotherObject->getMass() / (distance*distance);
double forceX = (anotherObject->getX() - getX()) / distance * force;
double forceY = (anotherObject->getY() - getY()) / distance * force;
double velX = getVelX();
double velY = getVelY();
velX += forceX / getMass() * delta;
velY += forceY / getMass() * delta;
setVelX(velX);
setVelY(velY);
}
void Club::findClosestPlayer()
{
float min_distance = 1000;
float distance_to_ball;
for (int i=0; i<11; i++)
{
distance_to_ball = distanceBetween( lineUp[i]->getPosition(),stadium->ball->getPosition2() );
if ( distance_to_ball<min_distance)
{
min_distance = distance_to_ball;
closestPlayer = lineUp[i];
}
}
//wcout << "closest: " << closestPlayer->get_footballer()->shortName_shown << endl;
};
void Camera::MoveEyeAlongGaze(
int from_x, int from_y, int to_x, int to_y, int imageWidth, int imageHeight)
{
ElVisFloat percent = static_cast<ElVisFloat>(from_y - to_y) /
static_cast<ElVisFloat>(imageHeight);
ElVisFloat startDistance = distanceBetween(m_eye, m_lookAt);
ElVisFloat endDistance = startDistance + percent * 5;
if (endDistance < m_near)
{
endDistance = m_near;
}
WorldVector reverseVector = -GetGaze();
reverseVector.Normalize();
m_eye = m_lookAt + findPointAlongVector(reverseVector, endDistance);
}
void Game::onPrimaryAction() {
if (selected != nullptr)
return;
bullet->setCenter(camera->getCenter());
while (nullptr == selected && distanceBetween(camera->getCenter(), bullet->getCenter()) < BULLET_DISTANCE) {
bullet->move(camera->getDirection());
for (auto block : extraBlocks) {
if (bullet->isColliding(*block)) {
block->setLocked(true);
selected = block;
break;
}
}
}
}
chemkit::Variant GraphDiameterDescriptor::value(const chemkit::Molecule *molecule) const
{
int diameter = 0;
for(size_t i = 0; i < molecule->size(); i++){
const chemkit::Atom *a = molecule->atom(i);
for(size_t j = i + 1; j < molecule->size(); j++){
const chemkit::Atom *b = molecule->atom(j);
int distance = distanceBetween(a, b);
if(distance > diameter){
diameter = distance;
}
}
}
return diameter;
}
void Game::playCutSceneLevelBeginning() {
if (cutSceneTimer.getTime() == 0) { // means running for 1st time
// move to level beginning -20 units, so player can see how platforms are being built
dummyCameraObject->moveTo(Point3f(1.0f, 45.0f, -currentLevel->length*1.0f - 20.0f));
dummyCameraObject->lookAt(prize->getCenter());
camera->follow(dummyCameraObject);
}
if (cutSceneTimer.getElapsed() >= 0.075 * __SECOND) {
buildPlatforms();
if (dummyCameraObject->getCenter().y > 7.0f) {
dummyCameraObject->move(Vector3f(0, -1.0f, 0.25f));
}
else {
if (distanceBetween(dummyCameraObject->getCenter(), prize->getCenter()) > 7.5f) {
dummyCameraObject->move(Vector3f(0, 0, 1.5f));
}
else {
cutSceneFrame++;
}
}
dummyCameraObject->lookAt(prize->getCenter());
cutSceneTimer.measure();
}
if (eventSystem->isPressed(Key::SPACE) || (isLevelBuilt() && cutSceneFrame > 0)) {
while (!isLevelBuilt()) {
buildPlatforms();
}
camera->follow(player);
resetCutScene();
spawnAllBlocks();
#ifdef __DEBUG
debug("Level Created, Blocks spawned, CutScene::LEVEL_BEGINNING completed");
#endif
}
}
double PairRelationDetector::TransPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0), min_dist, mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Eigen::Vector3d transVec = n1->center() - n2->center();
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(m2, transMat);
distanceBetween(m1, newverts2, min_dist, mean_dist, max_dist);
deviation = 2 * mean_dist / (n1->bbox().diagonal().norm() + n2->bbox().diagonal().norm());
if ( deviation > maxAllowDeviation_)
{
maxAllowDeviation_ = deviation;
}
return deviation;
}
void Game::buildBlock() {
if (nullptr == selected)
return;
std::list<Point3f>::iterator it = freeBlockSlots.begin();
while (it != freeBlockSlots.end()) {
if (distanceBetween(*it, selected->getCenter()) < 3.0f) {
selected->setCenter((*it).x, (*it).y, (*it).z);
selected->setLocked(false);
mainBlocks.push_back(selected);
extraBlocks.remove(selected);
it = freeBlockSlots.erase(it);
selected = nullptr;
player->addScore(SCORE_PER_BLOCK);
sfx->playSound(ResourceManager::getSound(_RES_SFX_CLONG));
return;
}
++it;
}
}
/* Find the new collision avoidance waypoint for the plane to go to */
au_uav_ros::waypoint au_uav_ros::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
au_uav_ros::waypoint wp;
double V = MPS_SPEED * MPS_WAYPOINT_MULTIPLIER;
double delta_T = TIME_STEP;
double cartBearing = plane1.getCurrentBearing()* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
V = V * MPS_WAYPOINT_MULTIPLIER;
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
ROS_INFO("long+%f, lat+%f, distanceBetween UAV and AvoidWP%f", V*cos(psi)/DELTA_LON_TO_METERS, V*sin(psi)/DELTA_LON_TO_METERS,
distanceBetween(plane1.getCurrentLoc(), wp));
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
double sadValue(cv::Point currentPosition, cv::Point position) {
double value = 0;
int row, column;
int fromX = -WINDOW_SIZE;
int fromY = -WINDOW_SIZE;
cv::Point from, to;
for(row = -WINDOW_SIZE; row <= WINDOW_SIZE; row++) {
for(column = -WINDOW_SIZE; column <= WINDOW_SIZE; column++) {
cv::Vec3b image0Value = image0.at<cv::Vec3b>(currentPosition.y + row, currentPosition.x + column);
cv::Vec3b image1Value = image1.at<cv::Vec3b>(position.y + row, position.x + column);
from = cv::Point(image0Value[1], image0Value[2]);
to = cv::Point(image1Value[1], image1Value[2]);
value += std::abs(distanceBetween(from, to));
}
}
return value;
}
