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; }