// Add intersections to queue
void AddIsects( const Ray3D &ray, const Box3D &box, PointQueue &isects )
{
const unsigned nPairs = 3;
Point3D rayEnd( ray.origin + ray.direction );
for ( unsigned i = 0; i < nPairs; ++i )
{
unsigned j = ( i + 1 ) % nPairs, k = ( i + 2 ) % nPairs;
if ( rayEnd[i] != ray.origin[i] )
{
for ( unsigned m = 0; m < 2; ++m)
{
float t = (!m) ? ( box.origin[i] - ray.origin[i] ) / ( ray.direction[i] )
: ( box.extent[i] - ray.origin[i] ) / ( ray.direction[i] );
if ( t >= -epsilon && t <= 1.f + epsilon )
{
Point3D isect = ray.origin + t * ray.direction;
if ( ( isect[j] >= box.origin[j] && isect[j] <= box.extent[j] ) &&
( isect[k] >= box.origin[k] && isect[k] <= box.extent[k] ) )
{
TimedPoint point = { t, isect };
if ( isects.c.end() == std::find( isects.c.begin(), isects.c.end(), point ) )
isects.push( point );
}
}
}
}
}
}
// Update live tracker data, non blocking
void LiveTrackerUpdate(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
livetracker_point_t newpoint;
static int logtime = 0;
if (!_inited) return; // Do nothing if not inited
if (Basic->NAVWarning) return; // Do not log if no gps fix
if (LiveTrackerInterval==0) return; // Disabled
CCriticalSection::CGuard guard(_t_mutex);
_t_barodevice = Basic->BaroDevice;
//Check if sending needed (time interval)
if (Basic->Time >= logtime) {
logtime = (int)Basic->Time + LiveTrackerInterval;
if (logtime>=86400) logtime-=86400;
} else return;
// Half hour FIFO must be enough
if (_t_points.size() > (unsigned int)(1800 / LiveTrackerInterval)) {
// points in queue are full, drop oldest point
_t_points.pop_front();
}
struct tm t;
time_t t_of_day;
t.tm_year = Basic->Year - 1900;
t.tm_mon = Basic->Month - 1; // Month, 0 - jan
t.tm_mday = Basic->Day;
t.tm_hour = Basic->Hour;
t.tm_min = Basic->Minute;
t.tm_sec = Basic->Second;
t.tm_isdst = 0; // Is DST on? 1 = yes, 0 = no, -1 = unknown
t_of_day = mkgmtime(&t);
newpoint.unix_timestamp = t_of_day;
newpoint.flying = Calculated->Flying;
newpoint.latitude = Basic->Latitude;
newpoint.longitude = Basic->Longitude;
newpoint.alt = Calculated->NavAltitude;
newpoint.ground_speed = Basic->Speed;
newpoint.course_over_ground = Calculated->Heading;
_t_points.push_back(newpoint);
SetEvent(_hNewDataEvent);
}
// Determines if 3D ray and AABB have a 0, 1, or 2 intersections.
// If any exist and rpoint is not NULL, returns intersection point(s).
int Intersects(const Ray3D& ray, const Box3D& box, std::pair<Point3D, Point3D> *rpoints)
{
PointQueue isects;
AddIsects( ray, box, isects );
std::sort( isects.c.begin(), isects.c.end(), TimedPointLess );
size_t nIsects = isects.size();
if ( rpoints )
{
for ( size_t i = 0; i < nIsects; ++i )
{
if ( i == 0 ) rpoints->first = isects.c[i].p;
else rpoints->second = isects.c[i].p;
}
}
return nIsects;
}
// Leonardo Live Tracker (www.livetrack24.com) data exchange thread
static void LiveTrackerThread()
{
int tracker_fsm = 0;
livetracker_point_t sendpoint = {0};
bool sendpoint_valid = false;
bool sendpoint_processed = false;
bool sendpoint_processed_old = false;
// Session variables
unsigned int packet_id = 0;
unsigned int session_id = 0;
int userid = -1;
_t_end = false;
_t_run = true;
srand(MonotonicClockMS());
do {
if (NewDataEvent.tryWait(5000)) NewDataEvent.reset();
if (!_t_run) break;
do {
if (1) {
sendpoint_valid = false;
ScopeLock guard(_t_mutex);
if (!_t_points.empty()) {
sendpoint = _t_points.front();
sendpoint_valid = true;
}
} //mutex
if (sendpoint_valid) {
sendpoint_processed = false;
do {
switch (tracker_fsm) {
default:
case 0: // Wait for flying
if (!sendpoint.flying) {
sendpoint_processed = true;
break;
}
tracker_fsm++;
break;
case 1:
// Get User ID
userid = GetUserIDFromServer();
sendpoint_processed = false;
if (userid>=0) tracker_fsm++;
break;
case 2:
//Start of track packet
sendpoint_processed = SendStartOfTrackPacket(&packet_id, &session_id, userid);
if (sendpoint_processed) {
StartupStore(TEXT(". Livetracker new track started.%s"),NEWLINE);
sendpoint_processed_old = true;
tracker_fsm++;
}
break;
case 3:
//Gps point packet
sendpoint_processed = SendGPSPointPacket(&packet_id, &session_id, &sendpoint);
//Connection lost to server
if (sendpoint_processed_old && !sendpoint_processed) {
StartupStore(TEXT(". Livetracker connection to server lost.%s"), NEWLINE);
}
//Connection established to server
if (!sendpoint_processed_old && sendpoint_processed) {
ScopeLock guard(_t_mutex);
int queue_size = _t_points.size();
StartupStore(TEXT(". Livetracker connection to server established, start sending %d queued packets.%s"), queue_size, NEWLINE);
}
sendpoint_processed_old = sendpoint_processed;
if (!sendpoint.flying) {
tracker_fsm++;
}
break;
case 4:
//End of track packet
sendpoint_processed = SendEndOfTrackPacket(&packet_id, &session_id);
if (sendpoint_processed) {
StartupStore(TEXT(". Livetracker track finished, sent %d points.%s"), packet_id, NEWLINE);
tracker_fsm=0;
}
break;
}// sw
if (sendpoint_processed) {
ScopeLock guard(_t_mutex);
_t_points.pop_front();
} else InterruptibleSleep(2500);
sendpoint_processed_old = sendpoint_processed;
} while (!sendpoint_processed && _t_run);
}
} while (sendpoint_valid && _t_run);
} while (_t_run);
_t_end = true;
}
// Leonardo Live Tracker (www.livetrack24.com) data exchange thread
static DWORD WINAPI LiveTrackerThread (LPVOID lpvoid)
{
int tracker_fsm = 0;
livetracker_point_t sendpoint = {0};
bool sendpoint_valid = false;
bool sendpoint_processed = false;
bool sendpoint_processed_old = false;
// Session variables
unsigned int packet_id = 0;
unsigned int session_id = 0;
int userid = -1;
_t_end = false;
_t_run = true;
srand(GetTickCount());
do {
if (WaitForSingleObject(_hNewDataEvent, 5000) == WAIT_OBJECT_0) ResetEvent(_hNewDataEvent);
if (!_t_run) break;
do {
if (1) {
sendpoint_valid = false;
CCriticalSection::CGuard guard(_t_mutex);
if (!_t_points.empty()) {
sendpoint = _t_points.front();
sendpoint_valid = true;
}
} //mutex
if (sendpoint_valid) {
sendpoint_processed = false;
do {
switch (tracker_fsm) {
default:
case 0: // Wait for flying
if (!sendpoint.flying) {
sendpoint_processed = true;
break;
}
tracker_fsm++;
break;
case 1:
// Get User ID
userid = GetUserIDFromServer();
sendpoint_processed = false;
if (userid>=0) tracker_fsm++;
break;
case 2:
//Start of track packet
sendpoint_processed = SendStartOfTrackPacket(&packet_id, &session_id, userid);
if (sendpoint_processed) {
StartupStore(TEXT(". Livetracker new track started.%s"),NEWLINE);
sendpoint_processed_old = true;
tracker_fsm++;
}
break;
case 3:
//Gps point packet
sendpoint_processed = SendGPSPointPacket(&packet_id, &session_id, &sendpoint);
//Connection lost to server
if (sendpoint_processed_old && !sendpoint_processed) {
StartupStore(TEXT(". Livetracker connection to server lost.%s"), NEWLINE);
}
//Connection established to server
if (!sendpoint_processed_old && sendpoint_processed) {
CCriticalSection::CGuard guard(_t_mutex);
int queue_size = _t_points.size();
StartupStore(TEXT(". Livetracker connection to server established, start sending %d queued packets.%s"), queue_size, NEWLINE);
}
sendpoint_processed_old = sendpoint_processed;
if (!sendpoint.flying) {
tracker_fsm++;
}
break;
case 4:
//End of track packet
sendpoint_processed = SendEndOfTrackPacket(&packet_id, &session_id);
if (sendpoint_processed) {
StartupStore(TEXT(". Livetracker track finished, sent %d points.%s"), packet_id, NEWLINE);
tracker_fsm=0;
}
break;
}// sw
if (sendpoint_processed) {
CCriticalSection::CGuard guard(_t_mutex);
_t_points.pop_front();
} else InterruptibleSleep(2500);
sendpoint_processed_old = sendpoint_processed;
} while (!sendpoint_processed && _t_run);
}
} while (sendpoint_valid && _t_run);
} while (_t_run);
_t_end = true;
return 0;
}
