imu_data_t* GladiatorIMU::acquireData() { /* First, pull a gladiator_rx_msg_t from the serial line, with uSync == RX_SYNC_BYTE */ int validMessage = 0; gladiator_rx_msg_t* fullMessage; imu_data_t *data; while (!validMessage) { fullMessage = getSingleMessage(); if (fullMessage) { data = translate(fullMessage); //We may have started with a message that doesn't include the scaling factors - loop around if (!epsilonEqual(accelScale, 0, FLOAT_EPSILON)) validMessage = 1; } } delete fullMessage; return data; }
int OGRLinearRing::isClockwise() const { int i, v, next; double dx0, dy0, dx1, dy1, crossproduct; if( nPointCount < 2 ) return TRUE; /* Find the lowest rightmost vertex */ v = 0; for ( i = 1; i < nPointCount - 1; i++ ) { /* => v < end */ if ( paoPoints[i].y< paoPoints[v].y || ( paoPoints[i].y== paoPoints[v].y && paoPoints[i].x > paoPoints[v].x ) ) { v = i; } } /* Vertices may be duplicate, we have to go to nearest different in each direction */ /* preceding */ next = v - 1; while ( 1 ) { if ( next < 0 ) { next = nPointCount - 1 - 1; } if( !epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) || !epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { break; } if ( next == v ) /* So we cannot get into endless loop */ { break; } next--; } dx0 = paoPoints[next].x - paoPoints[v].x; dy0 = paoPoints[next].y - paoPoints[v].y; /* following */ next = v + 1; while ( 1 ) { if ( next >= nPointCount - 1 ) { next = 0; } if ( !epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) || !epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { break; } if ( next == v ) /* So we cannot get into endless loop */ { break; } next++; } dx1 = paoPoints[next].x - paoPoints[v].x; dy1 = paoPoints[next].y - paoPoints[v].y; crossproduct = dx1 * dy0 - dx0 * dy1; if ( crossproduct > 0 ) /* CCW */ return FALSE; else if ( crossproduct < 0 ) /* CW */ return TRUE; /* ok, this is a degenerate case : the extent of the polygon is less than EPSILON */ /* Try with Green Formula as a fallback, but this is not a guarantee */ /* as we'll probably be affected by numerical instabilities */ double dfSum = paoPoints[0].x * (paoPoints[1].y - paoPoints[nPointCount-1].y); for (i=1; i<nPointCount-1; i++) { dfSum += paoPoints[i].x * (paoPoints[i+1].y - paoPoints[i-1].y); } dfSum += paoPoints[nPointCount-1].x * (paoPoints[0].y - paoPoints[nPointCount-2].y); return dfSum < 0; }
int OGRLinearRing::isClockwise() const { if( nPointCount < 2 ) return TRUE; bool bUseFallback = false; // Find the lowest rightmost vertex. int v = 0; // Used after for. for( int i = 1; i < nPointCount - 1; i++ ) { // => v < end. if( paoPoints[i].y< paoPoints[v].y || ( paoPoints[i].y== paoPoints[v].y && paoPoints[i].x > paoPoints[v].x ) ) { v = i; bUseFallback = false; } else if( paoPoints[i].y == paoPoints[v].y && paoPoints[i].x == paoPoints[v].x ) { // Two vertex with same coordinates are the lowest rightmost // vertex. Cannot use that point as the pivot (#5342). bUseFallback = true; } } // Previous. int next = v - 1; if( next < 0 ) { next = nPointCount - 1 - 1; } if( epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) && epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { // Don't try to be too clever by retrying with a next point. // This can lead to false results as in the case of #3356. bUseFallback = true; } const double dx0 = paoPoints[next].x - paoPoints[v].x; const double dy0 = paoPoints[next].y - paoPoints[v].y; // Following. next = v + 1; if( next >= nPointCount - 1 ) { next = 0; } if( epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) && epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { // Don't try to be too clever by retrying with a next point. // This can lead to false results as in the case of #3356. bUseFallback = true; } const double dx1 = paoPoints[next].x - paoPoints[v].x; const double dy1 = paoPoints[next].y - paoPoints[v].y; const double crossproduct = dx1 * dy0 - dx0 * dy1; if( !bUseFallback ) { if( crossproduct > 0 ) // CCW return FALSE; else if( crossproduct < 0 ) // CW return TRUE; } // This is a degenerate case: the extent of the polygon is less than EPSILON // or 2 nearly identical points were found. // Try with Green Formula as a fallback, but this is not a guarantee // as we'll probably be affected by numerical instabilities. double dfSum = paoPoints[0].x * (paoPoints[1].y - paoPoints[nPointCount-1].y); for( int i = 1; i < nPointCount-1; i++ ) { dfSum += paoPoints[i].x * (paoPoints[i+1].y - paoPoints[i-1].y); } dfSum += paoPoints[nPointCount-1].x * (paoPoints[0].y - paoPoints[nPointCount-2].y); return dfSum < 0; }
int OGRLinearRing::isClockwise() const { int i, v, next; double dx0, dy0, dx1, dy1, crossproduct; int bUseFallback = FALSE; if( nPointCount < 2 ) return TRUE; /* Find the lowest rightmost vertex */ v = 0; for ( i = 1; i < nPointCount - 1; i++ ) { /* => v < end */ if ( paoPoints[i].y< paoPoints[v].y || ( paoPoints[i].y== paoPoints[v].y && paoPoints[i].x > paoPoints[v].x ) ) { v = i; bUseFallback = FALSE; } else if ( paoPoints[i].y == paoPoints[v].y && paoPoints[i].x == paoPoints[v].x ) { /* Two vertex with same coordinates are the lowest rightmost */ /* vertex! We cannot use that point as the pivot (#5342) */ bUseFallback = TRUE; } } /* previous */ next = v - 1; if ( next < 0 ) { next = nPointCount - 1 - 1; } if( epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) && epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { /* Don't try to be too clever by retrying with a next point */ /* This can lead to false results as in the case of #3356 */ bUseFallback = TRUE; } dx0 = paoPoints[next].x - paoPoints[v].x; dy0 = paoPoints[next].y - paoPoints[v].y; /* following */ next = v + 1; if ( next >= nPointCount - 1 ) { next = 0; } if( epsilonEqual(paoPoints[next].x, paoPoints[v].x, EPSILON) && epsilonEqual(paoPoints[next].y, paoPoints[v].y, EPSILON) ) { /* Don't try to be too clever by retrying with a next point */ /* This can lead to false results as in the case of #3356 */ bUseFallback = TRUE; } dx1 = paoPoints[next].x - paoPoints[v].x; dy1 = paoPoints[next].y - paoPoints[v].y; crossproduct = dx1 * dy0 - dx0 * dy1; if (!bUseFallback) { if ( crossproduct > 0 ) /* CCW */ return FALSE; else if ( crossproduct < 0 ) /* CW */ return TRUE; } /* ok, this is a degenerate case : the extent of the polygon is less than EPSILON */ /* or 2 nearly identical points were found */ /* Try with Green Formula as a fallback, but this is not a guarantee */ /* as we'll probably be affected by numerical instabilities */ double dfSum = paoPoints[0].x * (paoPoints[1].y - paoPoints[nPointCount-1].y); for (i=1; i<nPointCount-1; i++) { dfSum += paoPoints[i].x * (paoPoints[i+1].y - paoPoints[i-1].y); } dfSum += paoPoints[nPointCount-1].x * (paoPoints[0].y - paoPoints[nPointCount-2].y); return dfSum < 0; }