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
);
}
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 ) );
}
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 ) );
}
}
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(¤tPosition, &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;
}
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 );
}
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 );
}
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
}
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;
}
}
}
/** 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;
}
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 )
);
}
}
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
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;
}
}
}
Vector AutoMatrix::newRow () {
const size_t row = countRows();
upSize( row + 1, countColumns() );
return rowVector( row );
}
void Matrix::factorizeQRP ( Vector& tau, Permutation& permutation ) {
int sign;
AutoVector workspace( countColumns() );
gsl_linalg_QRPT_decomp( &matrix, &tau.vector, &permutation, &sign, &workspace.vector );
}
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;
}
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;
}
Vector AutoMatrix::newColumn () {
const size_t col = countColumns();
upSize( countRows(), col + 1 );
return columnVector( col );
}