bool WaypointLoaderNode::verifyFileConsistency(const char *filename)
{
  ROS_INFO("verify...");
  std::ifstream ifs(filename);

  if (!ifs)
    return false;

  FileFormat format = checkFileFormat(filename);
  ROS_INFO("format: %d", static_cast<FileFormat>(format));
  if (format == FileFormat::unknown)
  {
    ROS_ERROR("unknown file format");
    return false;
  }

  std::string line;
  std::getline(ifs, line);  // remove first line

  size_t ncol = format == FileFormat::ver1 ? 4 //x,y,z,velocity
              : format == FileFormat::ver2 ? 5 //x,y,z,yaw,velocity
              : countColumns(line);

  while (std::getline(ifs, line))  // search from second line
  {
    if (countColumns(line) != ncol)
      return false;
  }
  return true;
}
FileFormat WaypointLoaderNode::checkFileFormat(const char *filename)
{
  std::ifstream ifs(filename);

  if (!ifs)
  {
    return FileFormat::unknown;
  }

  // get first line
  std::string line;
  std::getline(ifs, line);

  // parse first line
  std::vector<std::string> parsed_columns;
  parseColumns(line, &parsed_columns);

  // check if first element in the first column does not include digit
  if (!std::any_of(parsed_columns.at(0).cbegin(), parsed_columns.at(0).cend(), isdigit))
  {
    return FileFormat::ver3;
  }

  // if element consists only digit
  int num_of_columns = countColumns(line);
  ROS_INFO("columns size: %d", num_of_columns);

  return ( num_of_columns == 3 ? FileFormat::ver1  // if data consists "x y z (velocity)"
         : num_of_columns == 4 ? FileFormat::ver2  // if data consists "x y z yaw (velocity)
                               : FileFormat::unknown
          );
}
Example #3
0
	Vector Matrix::subdiagonalVector ( const size_t offset ) {
		// To circumvent GSL limitation to allow 0-dimensional vectors
		// This use of static data relies on the immutability of 0-dim Vectors.
		static Vector nullDimVector( 0, NULL, 0 );
		if ( 0 == countColumns() ) return nullDimVector;
		return Vector( gsl_matrix_subdiagonal( &matrix, offset ) );
	}
Example #4
0
	Matrix Matrix::subMatrix ( const size_t row, const size_t col, const size_t rows, const size_t cols ) {
		// Circumvent GSL limitation to allow 0-row and/or 0-column matrices
		if ( 0 == rows || 0 == cols ) {
			assert( row + rows <= countRows() );
			assert( col + cols <= countColumns() );
			return Matrix( rows, cols, matrix.data + row * matrix.tda + col, matrix.tda );
		} else {
			return Matrix( gsl_matrix_submatrix( &matrix, row, col, rows, cols ) );
		}
	}
Example #5
0
int main(int argc, char *argv[])
{
    uint32_t i;
    if (argc > 1)
    {
        int dataDescriptor = open(argv[1], O_RDONLY);
        if (dataDescriptor >= 0)
        {
            struct stat dataStats;
            if (!fstat(dataDescriptor, &dataStats))
            {
                char *dataMapping = mmap(NULL, dataStats.st_size, PROT_READ, MAP_PRIVATE, dataDescriptor, 0);
                if (dataMapping != MAP_FAILED)
                {
                    char *endOfMap = dataMapping + dataStats.st_size;
                    char *currentPosition = dataMapping;
                    uint32_t numColumns = countColumns(currentPosition, endOfMap);
                    if (numColumns)
                    {
                        slice *tokens = NULL;
                        uint32_t numRows = 0;
                        uint32_t maxRowSize = STARTING_ROWSIZE;
                        slice **rows = malloc(sizeof(slice *) * maxRowSize);
                        if (rows)
                        {
                            while (parseLine(&currentPosition, &tokens, numColumns, endOfMap))
                            {
                                rows[numRows++] = tokens;
                                if (numRows == maxRowSize)
                                {
                                    maxRowSize *= 2;
                                    rows = realloc(rows, sizeof(slice *) * maxRowSize);
                                }
                            }
                            if (numRows > 1)
                            {
                                for (i = 0; i < numColumns; i++)
                                    printf("%s%.*s",((i == 0) ? "" : ",") , (int)rows[1][i].size, rows[1][i].string);
                                printf("\n");
                            }
                            // uncomment to have things cleanup as they exit...
/*                            for (uint32_t i = 0; i < numRows; i++)
                                free(rows[i]);
                            free(rows); */
                        }
                    }
//                    munmap(dataMapping, dataStats.st_size);
                }
            }
  //          close(dataDescriptor);
        }
    }

    return EXIT_SUCCESS;
}
Example #6
0
	void AutoMatrix::removeColumn ( const size_t col ) {
		const size_t
			rows = countRows(),
			cols = countColumns();
		for ( size_t i = col + 1; i < cols; ++i ) {
			const Vector colSource = columnVector( i );
			Vector colTarget = columnVector( i - 1 );
			colTarget.copy( colSource );
		}
		upSize( rows, cols - 1 );
	}
Example #7
0
	void AutoMatrix::removeRow ( const size_t row ) {
		const size_t
			rows = countRows(),
			cols = countColumns();
		for ( size_t i = row + 1; i < rows; ++i ) {
			const Vector rowSource = rowVector( i );
			Vector rowTarget = rowVector( i - 1 );
			rowTarget.copy( rowSource );
		}
		upSize( rows - 1, cols );
	}
Example #8
0
	void AutoMatrix::exactSize ( const size_t rows, const size_t cols ) {
		const size_t required = calculateSize( rows, cols );
		double *newArray;
		if ( required == size ) {	// Shortcut: no malloc necessary
			if ( cols <= matrix.tda ) {	// move towards begin
				const size_t currentRows = countRows();
				const size_t blockSize = cols * sizeof( double );
				const double *source = matrix.data;
				double *target = matrix.data;
				for ( size_t row = 0; ++row < currentRows; ) {
					// For row = 0, no copy is needed, so the pre-increment of row and pointers is what we want
					memmove( target += cols, source += matrix.tda, blockSize );
				}
			} else {	// move towards end requires opposite loop direction (new cols bigger, so new rows smaller)
				const size_t currentCols = countColumns();
				const size_t blockSize = currentCols * sizeof( double );
				const double *source = &matrix.data[ matrix.tda * rows ];
				double *target = &matrix.data[ rows * cols ];
				// Guard against unsigned underflow
				if ( 0 < rows ) {
					for ( size_t row = rows; --row >= 1; ) {
						memmove( target -= cols, source -= matrix.tda, blockSize );
					}
				}
			}
			newArray = matrix.data;
		} else {
			newArray = static_cast<double*>( malloc( required ) );
			assert( NULL != newArray );
			const size_t copyRows = min( rows, countRows() );
			const size_t blockSize = min( cols, countColumns() ) * sizeof( double );
			for ( size_t row = 0; row < copyRows; ++row ) {
				memcpy( &newArray[ cols * row ], &matrix.data[ matrix.tda * row ], blockSize );
			}
			free( matrix.data );
			size = required;
		}
		gslInit( rows, cols, newArray, cols );	// update GSL struct variables
	}
Example #9
0
	void Matrix::fill ( const double *array ) {
		const size_t cols = countColumns();
		const size_t rows = countRows();
		if ( matrix.tda == cols || 1 >= rows ) {	// packed is copied as one lump
			const size_t blockSize = rows * cols * sizeof( double );
			memcpy( matrix.data, array, blockSize );
		} else {
			const size_t blockSize = cols * sizeof( double );
			double *target = matrix.data;
			for ( size_t row = 0; row < rows; ++row ) {
				memcpy( target, array, blockSize );
				array += cols;
				target += matrix.tda;
			}
		}
	}
Example #10
0
	/** Note that GSL has rows and columns in C convention ordering. */
	bool Matrix::gslInit ( const size_t rows, const size_t cols, double *base, const size_t tda ) {
		bool invalidates
			= base != matrix.data
			|| rows < countRows()
			|| cols < countColumns()
			|| ( tda != matrix.tda && 1 < countRows() );
		// Circumvent GSL limitation to allow 0-row and/or 0-column matrices
		if ( 0 == rows || 0 == cols ) {
			assert( 0 <= rows );
			assert( 0 <= cols );
			assert( cols <= tda );
			matrix.data = base;
			matrix.size1 = rows;
			matrix.size2 = cols;
			matrix.tda = tda;
		} else {
			*static_cast<gsl_matrix_view*>( this ) = gsl_matrix_view_array_with_tda( base, rows, cols, tda );
		}
		return invalidates;
	}
Example #11
0
	void Matrix::extractQ ( const Vector& tau, const Matrix& qr ) {
		const size_t
			rows = countRows(),
			cols = countColumns(),
			qrRows = qr.countRows(),
			qrCols = qr.countColumns(),
			tauDims = tau.countDimensions();
		assert( qrRows >= qrCols );
		assert( qrCols == tauDims );
		assert( rows == qrRows );
		assert( cols == qrCols || cols == qrRows );
		fill( 0.0 );
		Vector diagonal = diagonalVector();
		diagonal.fill( 1.0 );
		for ( size_t col = qrCols; 0 < col--; ) {
			Matrix affected = subMatrix( col, col, rows - col, cols - col );
			affected.householderTransform(
				tau.get( col ),
				const_cast<Matrix&>( qr ).columnVector( col ).subVector( col, rows - col )
			);
		}
	}
Example #12
0
void EmacsBuffer::forcePointInWindow(mintcount_t li, mintcount_t co,
                                     mintcount_t tp, mintcount_t bp) {
    mintcount_t tl = li * tp / 100;
    if (_pointLine <= tl) {
        // Inside top margin
        _topline = 0;
        _toplineLine = 0;
    } else {
        mintcount_t bl = li * bp / 100;
        if (_pointLine >= _countNewlines - bl) {
            // Inside bottom margin
            _topline = backwardLines(getMarkPosition(MARK_BOL, _text.size()), li - 1);
            _toplineLine = _countNewlines - (li - 1);
        } else {
            if (_pointLine < (_toplineLine + tl)) {
                // Point is before top margin
                mintcount_t blines = (_toplineLine + tl) - _pointLine;
                _topline = backwardLines(_topline, blines);
                _toplineLine -= blines;
            } else if (_pointLine >= (_toplineLine + (li - bl))) {
                // Point is after bottom margin
                mintcount_t flines = _pointLine - (_toplineLine + (li - bl));
                _topline = forwardLines(_topline, flines);
                _toplineLine += flines;
            } // else if
        } // else
    } // else

    // Now the point line is in the window.  Do the same for point column.
    mintcount_t point_column = countColumns(getMarkPosition(MARK_BOL, _point), _point);
    if (point_column < _leftcol) {
        // point is off the left edge of the screen
        _leftcol = (point_column / _tab_width) * _tab_width;
    } else while (point_column >= (_leftcol + co)) {
        // point is off the right edge of the screen
        _leftcol += _tab_width;
    } // else if
} // EmacsBuffer::forcePointInWindow
Example #13
0
	AutoMatrix::AutoMatrix ( const AutoMatrix& original ) : Matrix(
		original.countRows(),
		original.countColumns(),
		static_cast<double*>( malloc( original.size ) ),
		original.matrix.tda
	), size( original.size ) {
		const size_t
			rows = countRows(),
			cols = countColumns();
		if ( cols == matrix.tda ) {
			// copy one big lump
			memcpy( matrix.data, original.matrix.data, calculateSize( rows, cols ) );
		} else {
			// copy row by row
			const size_t blockSize = cols * sizeof( double );
			const double *source = original.matrix.data;
			double *target = matrix.data;
			for ( size_t row = 0; row < rows; ++row ) {
				memcpy( target, source, blockSize );
				source += original.matrix.tda;
				target += matrix.tda;
			}
		}
	}
Example #14
0
	Vector AutoMatrix::newRow () {
		const size_t row = countRows();
		upSize( row + 1, countColumns() );
		return rowVector( row );
	}
Example #15
0
	void Matrix::factorizeQRP ( Vector& tau, Permutation& permutation ) {
		int sign;
		AutoVector workspace( countColumns() );
		gsl_linalg_QRPT_decomp( &matrix, &tau.vector, &permutation, &sign, &workspace.vector );
	}
Example #16
0
struct customTrack *chromGraphParser(char *genomeDb, struct customPp *cpp,
	char *formatType, char *markerType, char *columnLabels,
	char *name, char *description, struct hash *settings,
	boolean report)
/* Parse out a chromGraph file (not including any track lines) */
{
char *minVal = hashFindVal(settings, "minVal");
char *maxVal = hashFindVal(settings, "maxVal");

/* Get first lines of track.  If track empty then
 * might as well return NULL here. */
struct slName *preview = getPreview(cpp, 10);
if (preview == NULL)
    return NULL;

/* Figure out format type - scanning preview if it isn't well defined. */
struct sqlConnection *conn = hAllocConn(genomeDb);
int colCount;

if (sameString(formatType, cgfFormatGuess))
    {
    if (!figureOutFormat(preview, &formatType, &colCount))
	errAbort("Can't figure out format for chromGraph track %s", 
		emptyForNull(name));
    }
hashMayRemove(settings, "formatType");

/* Now that we know format can count columns and determine how to
 * chop up lines. */
colCount = countColumns(preview, formatType);
Chopper chopper = getChopper(formatType);

/* Figure out marker type - scanning marker column of preview if it isn't
 * well defined. */
if (sameString(markerType, cgfMarkerGuess))
    {
    markerType = guessMarkerType(preview, chopper, conn, colCount);
    if (markerType == NULL)
	errAbort("Can't figure out marker column type for chromGraph track %s",
		emptyForNull(name));
    }
hashMayRemove(settings, "markerType");

/* Figure out if columns are labeled in file, using preview if needed. */
if (sameString(columnLabels, cgfColLabelGuess))
    {
    if (firstRowConsistentWithData(preview->name, chopper, colCount))
	columnLabels = cgfColLabelNumbered;
    else
	columnLabels = cgfColLabelFirstRow;
    }
hashMayRemove(settings, "columnLabels");
returnPreview(cpp, &preview);
boolean labelsInData = sameString(columnLabels, cgfColLabelFirstRow);

/* Load data into list of labeled temp files. */
struct labeledFile *fileEl, *fileList;
fileList = parseToLabeledFiles(cpp, colCount, formatType, markerType,
    labelsInData, conn, report);
saveLabeledFileList(fileList);

/* Create a customTrack for each element in file list. */
struct customTrack *outputTracks = NULL;
for (fileEl = fileList; fileEl != NULL; fileEl = fileEl->next)
    {
    struct customTrack *track;
    AllocVar(track);
    struct trackDb *tdb = customTrackTdbDefault();
    track->tdb = tdb;

    /* Figure out short and long names and type*/
    char shortLabel[128];
    char longLabel[512];
    if (name == NULL)
        name = track->tdb->shortLabel;
    if (description == NULL)
        description = track->tdb->longLabel;
    if (colCount > 1 || labelsInData)
        {
	safef(shortLabel, sizeof(shortLabel), "%s %s", name, fileEl->label);
	safef(longLabel, sizeof(longLabel), "%s %s", description, fileEl->label);
	}
    else
        {
	safef(shortLabel, sizeof(shortLabel), "%s", name);
	safef(longLabel, sizeof(longLabel), "%s", description);
	}
    tdb->shortLabel = cloneString(shortLabel);
    tdb->longLabel = cloneString(longLabel);
    tdb->type = "chromGraph";
    tdb->track = customTrackTableFromLabel(tdb->shortLabel);
    tdb->table = cloneString(tdb->track);
    track->dbTableName = NULL;

    /* Create settings */
    struct dyString *dy = dyStringNew(0);
    dyStringAppend(dy, hashToRaString(settings));
    dyStringPrintf(dy, "binaryFile %s\n", fileEl->fileName);
    dyStringPrintf(dy, "type %s\n", tdb->type);
    dyStringPrintf(dy, "tdbType %s\n", tdb->type); /* Needed if outside factory */
    if (minVal == NULL)
        dyStringPrintf(dy, "minVal %g\n", fileEl->minVal);
    if (maxVal == NULL)
        dyStringPrintf(dy, "maxVal %g\n", fileEl->maxVal);
    tdb->settings = dyStringCannibalize(&dy);
    
    /* Add to list. */
    slAddHead(&outputTracks, track);
    }
hFreeConn(&conn);
slReverse(&outputTracks);
return outputTracks;
}
Example #17
0
main(int argc, char *argv[]) {

   int rows = countRows(argv[1]);
   int columns = countColumns(argv[1]);
   
   printf("Rows   : %d\n", rows);
   printf("Columns: %d\n", columns);
   
   float tableau[rows][columns];

   /* DIAGNOSTICS */

   int i,j;
   int count = 1; // contatore Tableau

   /* Print Tableau */
   printf("\n=== PRINT TABLEAU BEFORE IMPORT [MAIN] ===\n");
   for (i = 0; i < rows; i++) {
      for (j = 0; j < columns; j++)
         printf("(%d,%d): %.3f\t", i, j, tableau[i][j]);
      printf("\n");
   }

   printf("\nTableau Address: %p\n", tableau);

   /* Memory Addresses */
   printf("\n=== MEMORY ADDRESSES BEFORE IMPORT [MAIN]===\n");
   for (i = 0; i < rows; i++) {
      for (j = 0; j < columns; j++)
         printf("(%d,%d): %p\t", i, j, &(tableau[i][j]));
      printf("\n");
   }

   import(rows,columns,tableau);

   /* Memory Addresses */
   printf("\n=== MEMORY ADDRESSES AFTER IMPORT [MAIN]===\n");
   for (i = 0; i < rows; i++) {
      for (j = 0; j < columns; j++)
         printf("(%d,%d): %p\t", i, j, &(tableau[i][j]));
      printf("\n");
   }

   /* Print Tableau */
   printf("\n=== PRINT TABLEAU AFTER IMPORT [MAIN] ===\n");
   for (i = 0; i < rows; i++) {
      for (j = 0; j < columns; j++)
         printf("(%d,%d): %.3f\t", i, j, tableau[i][j]);
      printf("\n");
   }

   /* === SIMPLEX === */

   int unbounded = 0;
   int optimal = 0;

   float theta,ratio;

   int k,h,sw;

   while ((optimal < (columns - 1)) && (unbounded < (rows - 1))) {
      for (j = 1; j < columns; j++)
         if (tableau[0][j] >= 0)
            optimal++;  // se tutti i costi ridotti sono > 0, allora siamo nell'ottimo
         else { // altrimenti scelgo una variabile fuori base con costo ridotto < 0 (Bland)
            optimal = 0;
            h = j; // salvo l'indice della colonna della variabile fuori base scelta
            for (i = 1; i < rows; i++)
               if (tableau[i][h] <= 0) // se ogni riga della colonna della variabile fuori base e' <= 0, allora e' illimitato
                  unbounded++;
               else {
                  unbounded = 0;
                  sw = 0;
                  for (i = 1; i < rows; i++) { // altrimenti calcola i rapporti ed il theta
                     if (tableau[i][h] > 0 && sw == 0) {
                        theta = tableau[i][0] / tableau[i][h];
                        k = i;
                        sw++;
                     }
                     else if (tableau[i][h] > 0) {
                       ratio = tableau[i][0] / tableau[i][h];
                       if (ratio < theta) {
                          theta = ratio;
                          k = i;
                       }
                     }    
                  }
               }
            
            pivot(k, h, rows, columns, tableau);
            
            printf("\n=== PRINT TABLEAU %d ===\n", count++);
            for (i = 0; i < rows; i++) {
               for (j = 0; j < columns; j++)
                  printf("(%d,%d): %.3f\t", i, j, tableau[i][j]);
               printf("\n");
            }
         }    
   }

   return 0;
}
Example #18
0
	Vector AutoMatrix::newColumn () {
		const size_t col = countColumns();
		upSize( countRows(), col + 1 );
		return columnVector( col );
	}