bool Plane::isIntersectionLine( const Line3& line ) const {
// Note: this tests if a line intersects the plane, not whether it (or its end-points) are coplanar with it.
auto startSign = distanceToPoint( line.start );
auto endSign = distanceToPoint( line.end );
return ( startSign < 0 && endSign > 0 ) || ( endSign < 0 && startSign > 0 );
}
double TupGraphicalAlgorithm::angleForPos(const QPointF &pos, const QPointF &anchor)
{
qreal distance = distanceToPoint(pos - anchor);
qreal angle = 0;
if (distance != 0) {
angle = ::acos((pos.x()-anchor.x()) / distanceToPoint(pos - anchor));
if (pos.y()-anchor.y() > 0)
angle = M_PI * 2.0 - angle;
}
return angle;
}
Vector3& Plane::orthoPoint( const Vector3& point, Vector3& target ) {
auto perpendicularMagnitude = distanceToPoint( point );
return target.copy( normal ).multiplyScalar( perpendicularMagnitude );
}
Vector3 Plane::orthoPoint( const Vector3& point) const {
auto perpendicularMagnitude = distanceToPoint( point );
return Vector3().copy( normal ).multiplyScalar( perpendicularMagnitude );
}
QPair<double, QPointF> GLEDrawingObject::updateOTracks(QPointF cursorPosition)
{
QPair<double, QPointF> nearestPoint;
nearestPoint.first = 1e6;
nearestPoint.second = QPointF(0.0,0.0);
for (int i=otrackList.size()-1;i>=0;i--)
{
double dist = otrackList[i]->distanceToPoint(cursorPosition, &nearestPoint.second);
double objectDistance = distanceToPoint(cursorPosition);
if ((dist > MAX_SNAP_LINE_DISTANCE) && (objectDistance > MAX_SNAP_LINE_DISTANCE))
{
if (QGLE::distance(cursorPosition,
otrackList[i]->getQtPoint(SnapLine::StartPoint)) > 3*MAX_SNAP_LINE_DISTANCE)
otrackList[i]->deactivate();
else
{
delete otrackList[i];
otrackList.removeAt(i);
}
}
else
{
otrackList[i]->activate();
nearestPoint.first = dist;
}
}
return(nearestPoint);
}
optional<Vector3> Plane::intersectLine( const Line3& line, Vector3& target ) {
auto direction = line.delta();
auto denominator = normal.dot( direction );
if ( denominator == 0 ) {
// line is coplanar, return origin
if( distanceToPoint( line.start ) == 0 ) {
target.copy( line.start );
return target;
}
// Unsure if this is the correct method to handle this case.
return nullptr;
}
auto t = - ( line.start.dot( normal ) + constant ) / denominator;
if( t < 0 || t > 1 ) {
return nullptr;
}
return target.copy( direction ).multiplyScalar( t ).add( line.start );
}
bool Plane::intersectLine(const Line3& line, Vector3* result) const
{
const Vector3 direction = line.delta();
float denominator = normal().dot( direction );
if ( denominator == 0 ) {
// line is coplanar, return origin
if ( distanceToPoint( line.start() ) == 0 ) {
if (result) {
result->copy( line.start() );
}
return true;
}
// Unsure if this is the correct method to handle this case.
return false;
}
float t = - ( line.start().dot( normal() ) + constant() ) / denominator;
if( (t < 0 || t > 1) ) {
return false;
}
if (result)
result->copy( direction ).multiplyScalar( t ).add( line.start() );
return true;
}
Vector3 Plane::orthoPoint( const Vector3& point ) const
{
float perpendicularMagnitude = distanceToPoint( point );
Vector3 result;
return result.copy( normal() ).multiplyScalar( perpendicularMagnitude );
}
float BezierCurve::distanceToPoint(const Vector3F & toP, Vector3F & closestP) const
{
float minD = 1e8;
const unsigned ns = numSegments();
for(unsigned i=0; i < ns; i++) {
BezierSpline sp;
getSegmentSpline(i, sp);
distanceToPoint(sp, toP, minD, closestP);
}
return minD;
}
bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p,
double distance_threshold,
double& output_distance)
{
// first check distance as Plane rather than Convex
double plane_distance = distanceToPoint(p);
if (plane_distance > distance_threshold) {
output_distance = plane_distance;
return false;
}
Eigen::Vector3f foot_point;
project(p, foot_point);
double convex_distance = (p - foot_point).norm();
output_distance = convex_distance;
return convex_distance < distance_threshold;
}
bool Sector::isPointInSector(const Math::Vector3d &point) const {
// Calculate the distance of the point from the plane of the sector.
// Return false if it isn't within a margin.
if (_height < 9000.f) { // No need to check when height is 9999.
float dist = distanceToPoint(point);
if (fabsf(dist) > _height + 0.01) // Add an error margin
return false;
}
// On the plane, so check if it is inside the polygon.
for (int i = 0; i < _numVertices; i++) {
Math::Vector3d edge = _vertices[i + 1] - _vertices[i];
Math::Vector3d delta = point - _vertices[i];
Math::Vector3d cross = Math::Vector3d::crossProduct(edge, delta);
if (cross.dotProduct(_normal) < -0.000001f) // not "< 0.f" here, since the value could be something like -7.45058e-09 and it
return false; // shuoldn't return. that was causing issue #610 (infinite loop in de.forklift_actor.dismount)
}
return true;
}
double Line::distanceToPoint(const Eigen::Vector3d& from)
{
Eigen::Vector3d foot_point;
return distanceToPoint(from, foot_point);
}
double Vector3D::distanceToLine(const Vector3D &point, const Vector3D &direction) const
{
double t= (point*direction-(*this)*direction)/(direction*direction);
return distanceToPoint(point+t*direction);
}
void GameState::update()
{
if (mgr->input->isKeyDown(ALLEGRO_KEY_LEFT))
{
player->turnLeft(degToRad(5));
}
else if (mgr->input->isKeyDown(ALLEGRO_KEY_RIGHT))
{
player->turnRight(degToRad(5));
}
player->update();
if (player->getX() < 0) player->setX(WIDTH);
else if (player->getX() > WIDTH) player->setX(0);
if (player->getY() < 0) player->setY(HEIGHT);
else if (player->getY() > HEIGHT) player->setY(0);
for (int i = 0, j = 0; i < NUM_ASTEROIDS; i++, j++)
{
// UPDATE BULLETS
if (j < NUM_BULLETS)
{
if (bullets[j].getAlive())
{
bullets[j].update();
if (bullets[j].getX() < 0 || bullets[j].getX() > WIDTH || bullets[j].getY() < 0 || bullets[j].getY() > HEIGHT)
{
bullets[j].kill();
}
}
}
// UPDATE ASTEROIDS
if (!asteroids[i].getAlive()) continue;
// CHECK COLLISIONS
if (player->isAlive())
{
if (distanceToPoint(asteroids[i].getX(), asteroids[i].getY(), player->getX(), player->getY()) < asteroids[i].getSpriteWidth()/2)
{
player->respawn(WIDTH/2, HEIGHT/2); // PLAYER COLLIDES WITH ASTEROID
player->setAlive(false);
mgr->sound->playSound(explosion1, 1);
lives--;
if (lives < 0)
{
data.time = timer.getSeconds();
mgr->states->changeState(new NameState(mgr, data));
return;
}
if (data.score >= 500) data.score -= 500;
else data.score = 0;
}
}
asteroids[i].update();
for (int b = 0; b < NUM_BULLETS; b++)
{
if (bullets[b].getAlive())
{
if (distanceToPoint(bullets[b].getX(), bullets[b].getY(), asteroids[i].getX(), asteroids[i].getY()) < asteroids[i].getSpriteWidth()/2)
{
bullets[b].kill(); // BULLET COLLIDES WITH ASTEROID
if (asteroids[i].getLarge())
data.score += 25;
else data.score += 50;
switch (rand()%4)
{
case 0:
mgr->sound->playSound(explosion1, 1);
break;
case 1:
mgr->sound->playSound(explosion2, 1);
break;
case 2:
mgr->sound->playSound(explosion3, 1);
break;
case 3:
mgr->sound->playSound(explosion4, 1);
break;
default:
break;
}
if (asteroids[i].getLarge()) // SPAWN LITTLE ASTEROIDS FROM BIG ASTEROID
{
int num = rand() % 1 + 2;
int count = 0;
for (int k = 0; k < NUM_ASTEROIDS; k++)
{
if (count > num) break;
if (asteroids[k].getAlive()) continue;
else
{
asteroids[k].spawn(asteroids[i].getX(), asteroids[i].getY(), false);
count++;
}
}
asteroids[i].despawn();
}
else // despawn little asteroids
{
asteroids[i].despawn();
spawnAsteroids(i);
}
}
}
}
}
if (_time >= timeBetweenAsteroids - (timer.getSeconds()/30))
{
for (int i = 0; i < NUM_ASTEROIDS; i++)
{
if (!asteroids[i].getAlive())
{
spawnAsteroids(i);
break;
}
}
_time = 0;
}
_time++;
}
float Plane::distanceToSphere( const Sphere& sphere ) const {
return distanceToPoint( sphere.center ) - sphere.radius;
}
double GLEText::nearestOSnap(const QPointF& p, QPointF *osnap)
{
return distanceToPoint(p, osnap);
}
static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user)
{
// Handle param messages
paramClient->handleMAVLinkPacket(msg);
//check for timed-out operations
struct timeval tv;
gettimeofday(&tv, NULL);
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state);
current_state = PX_WPP_IDLE;
protocol_current_count = 0;
protocol_current_partner_systemid = 0;
protocol_current_partner_compid = 0;
protocol_current_wp_id = -1;
if(waypoints->size() == 0)
{
current_active_wp_id = -1;
}
}
if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size())
{
send_setpoint(current_active_wp_id);
}
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE");
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
yawReached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
yawReached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
yawReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos);
if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
posReached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
if (wp->param2 == 0)
{
dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
}
else
{
dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
}
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_CMD: // special action from ground station
{
mavlink_cmd_t action;
mavlink_msg_cmd_decode(msg, &action);
if(action.target == systemid)
{
if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
switch (action.action)
{
// case MAV_ACTION_LAUNCH:
// if (verbose) std::cerr << "Launch received" << std::endl;
// current_active_wp_id = 0;
// if (waypoints->size()>0)
// {
// setActive(waypoints[current_active_wp_id]);
// }
// else
// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// break;
// case MAV_ACTION_CONTINUE:
// if (verbose) std::c
// err << "Continue received" << std::endl;
// idle = false;
// setActive(waypoints[current_active_wp_id]);
// break;
// case MAV_ACTION_HALT:
// if (verbose) std::cerr << "Halt received" << std::endl;
// idle = true;
// break;
// default:
// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// break;
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
mavlink_waypoint_ack_t wpa;
mavlink_msg_waypoint_ack_decode(msg, &wpa);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid))
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (protocol_current_wp_id == waypoints->size()-1)
{
if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
current_state = PX_WPP_IDLE;
protocol_current_wp_id = 0;
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
mavlink_waypoint_set_current_t wpc;
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE)
{
if (wpc.seq < waypoints->size())
{
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (i == current_active_wp_id)
{
waypoints->at(i)->current = true;
}
else
{
waypoints->at(i)->current = false;
}
}
if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
mavlink_waypoint_request_list_t wprl;
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
if(wprl.target_system == systemid && wprl.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST)
{
if (waypoints->size() > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
current_state = PX_WPP_SENDLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
}
else
{
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
protocol_current_count = waypoints->size();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
mavlink_waypoint_request_t wpr;
mavlink_msg_waypoint_request_decode(msg, &wpr);
if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid)
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
{
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
current_state = PX_WPP_SENDLIST_SENDWPS;
protocol_current_wp_id = wpr.seq;
send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
}
else
{
if (verbose)
{
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
else if (current_state == PX_WPP_SENDLIST)
{
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
}
else if (current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
mavlink_waypoint_count_t wpc;
mavlink_msg_waypoint_count_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0))
{
if (wpc.count > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
protocol_current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints_receive_buffer->back();
waypoints_receive_buffer->pop_back();
}
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
else if (wpc.count == 0)
{
printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
break;
}
else
{
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
}
}
else
{
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT:
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid))
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
{
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1;
mavlink_waypoint_t* newwp = new mavlink_waypoint_t;
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
waypoints_receive_buffer->push_back(newwp);
if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
{
if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
if (current_active_wp_id > waypoints_receive_buffer->size()-1)
{
current_active_wp_id = waypoints_receive_buffer->size() - 1;
}
// switch the waypoints list
std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints;
waypoints = waypoints_receive_buffer;
waypoints_receive_buffer = waypoints_temp;
//get the new current waypoint
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (waypoints->at(i)->current == 1)
{
current_active_wp_id = i;
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
break;
}
}
if (i == waypoints->size())
{
current_active_wp_id = -1;
yawReached = false;
posReached = false;
timestamp_firstinside_orbit = 0;
}
current_state = PX_WPP_IDLE;
}
else
{
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
}
else
{
if (current_state == PX_WPP_IDLE)
{
//we're done receiving waypoints, answer with ack.
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
}
if (verbose)
{
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
else if (current_state == PX_WPP_GETLIST)
{
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else if (current_state == PX_WPP_GETLIST_GETWPS)
{
if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
}
else if(wp.target_system == systemid && wp.target_component == compid)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
mavlink_waypoint_clear_all_t wpca;
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE)
{
protocol_timestamp_lastaction = now;
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
while(waypoints->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
}
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
}
break;
}
default:
{
if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl;
break;
}
}
//check if the current waypoint was reached
if ((posReached && /*yawReached &&*/ !idle))
{
if (current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id);
if (timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
send_waypoint_reached(cur_wp->seq);
timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
current_active_wp_id = 1;
}
else
{
if ((uint16_t)(current_active_wp_id + 1) < waypoints->size())
current_active_wp_id++;
}
// Fly to next waypoint
timestamp_firstinside_orbit = 0;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
waypoints->at(current_active_wp_id)->current = true;
posReached = false;
yawReached = false;
if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id);
}
}
}
}
else
{
timestamp_lastoutside_orbit = now;
}
}
double RansacCircleModel::calculateError(const DataPoint& p) const {
double error = distanceToPoint(p);
return error * error;
}
double RansacLineModel::calculateError(const DataPoint& p) const {
double val = distanceToPoint(std::forward<const DataPoint&>(p));
return val * val;
}
