void FileProjections::readProjectionsInfo()
{
// Prepare nodes
NodeMap* nodeList = graph->getNodeMap();
unsigned count = nodeList->size();
if (!count)
return;
unsigned oldSize = projectionsList->size();
bool isProjectionsWasEmpty = !oldSize;
projectionsList->resize(count, nullptr);
projectionsList->shrink_to_fit();
auto oldStart = projectionsList->begin();
auto oldEnd = projectionsList->begin() + oldSize;
auto it = nodeList->begin();
Projection* pr;
ProjectionsReader reader(*this);
std::string saveName(graphFileName);
unsigned nameSize = saveName.size();
bool someMissing = false, someReaded = false;
startProcess(0u, count - 1);
unsigned currentId;
unsigned pos = oldSize;
for(unsigned i = 0u; i < count; ++i, ++it)
{
if (isInterrupted())
{
projectionStatus = Status::PARTIAL;
return;
}
updateProgress(i);
currentId = it->first;
pr = nullptr;
// Projection exist - skip
if (!isProjectionsWasEmpty)
{
auto e = std::lower_bound(oldStart, oldEnd, currentId,
Projection::lessById);
if (e != oldEnd && (*e)->getId() == currentId)
{
pr = *e;
}
}
if (!pr)
{
pr = new Projection(currentId);
(*projectionsList)[pos] = pr;
++pos;
}
ProjectionsReader::projectionFileName(saveName, nameSize, currentId);
bool result = reader.readProjectionInfo(saveName.data(), pr);
if (result)
{
someReaded = true;
}
else
{
someMissing = true;
if (reader.getLastError() == ProjectionsReader::Error::TYPE)
{
lastError = Error::TYPE;
}
}
}
if (someReaded)
{
projectionStatus = someMissing ? Status::PARTIAL : Status::ALL;
}
else
{
projectionStatus = Status::EMPTY;
}
// check that list was not empty and some new projections added
if (oldSize && oldSize != count)
{
auto end = projectionsList->end();
std::sort(oldStart, end, Projection::less);
}
completeProcess();
}
void FileProjections::createAllProjections()
{
if (graph->isEmpty())
{
return;
}
if(isMemoryUsed())
{
Projections::createAllProjections();
return;
}
// Prepare nodes
NodeMap* nodeList = graph->getNodeMap();
unsigned count = nodeList->size();
if (!count)
return;
ProjectionsWriter writer(*this);
setWorker(&writer, true);
startProcess(0u, count - 1);
unsigned oldSize = projectionsList->size();
bool isWasEmpty = !oldSize;
projectionsList->resize(count, nullptr);
projectionsList->shrink_to_fit();
auto oldStart = projectionsList->begin();
auto oldEnd = projectionsList->begin() + oldSize;
auto it = nodeList->begin();
Projection* pr;
std::string saveName(graphFileName);
unsigned nameSize = saveName.size();
unsigned currentId;
unsigned pos = oldSize;
for(unsigned i = 0u; i < count; ++i, ++it)
{
if (isInterrupted())
{
projectionStatus = Status::PARTIAL;
// if interrupted on null filled projections, shrink list
projectionsList->resize(pos);
break;
}
updateProgress(i);
currentId = it->first;
pr = nullptr;
// Projection exist - skip
if (!isWasEmpty)
{
auto e = std::lower_bound(oldStart, oldEnd, currentId,
Projection::lessById);
if (e != oldEnd && (*e)->getId() == currentId)
{
pr = *e;
if (pr->fileExist())
continue;
}
}
if (!pr)
{
pr = new Projection(currentId);
(*projectionsList)[pos] = pr;
++pos;
}
pr->createProjection(*graph);
if (pr->isInterrupted())
{
continue;
}
ProjectionsReader::projectionFileName(saveName, nameSize, currentId);
bool result = writer.saveProjection(saveName.data(), pr);
if (result)
{
pr->setFileExist(true);
}
// stay loaded last projection
if (count - i > 1)
{
pr->clear();
}
else
{
loadedProjection = pr;
}
}
if (!isWasEmpty)
{
auto end = projectionsList->end();
std::sort(oldStart, end, Projection::less);
}
if (!isInterrupted())
projectionStatus = Status::ALL;
completeProcess();
}
void Cvode::CVodeCore()
{
_idid = CVodeReInit(_cvodeMem, _tCurrent, _CV_y);
_idid = CVodeSetStopTime(_cvodeMem, _tEnd);
_idid = CVodeSetInitStep(_cvodeMem, 1e-12);
if (_idid < 0)
throw ModelicaSimulationError(SOLVER,"CVode::ReInit");
bool writeEventOutput = (_settings->getGlobalSettings()->getOutputPointType() == OPT_ALL);
bool writeOutput = !(_settings->getGlobalSettings()->getOutputPointType() == OPT_NONE);
while ((_solverStatus & ISolver::CONTINUE) && !_interrupt )
{
_cv_rt = CVode(_cvodeMem, _tEnd, _CV_y, &_tCurrent, CV_ONE_STEP);
_idid = CVodeGetNumSteps(_cvodeMem, &_locStps);
if (_idid != CV_SUCCESS)
throw ModelicaSimulationError(SOLVER,"CVodeGetNumSteps failed. The cvode mem pointer is NULL");
_idid = CVodeGetLastStep(_cvodeMem, &_h);
if (_idid != CV_SUCCESS)
throw ModelicaSimulationError(SOLVER,"CVodeGetLastStep failed. The cvode mem pointer is NULL");
//set completed step to system and check if terminate was called
if(_continuous_system->stepCompleted(_tCurrent))
_solverStatus = DONE;
//Check if there was at least one output-point within the last solver interval
// -> Write output if true
if (writeOutput)
{
writeCVodeOutput(_tCurrent, _h, _locStps);
}
#ifdef RUNTIME_PROFILING
MEASURETIME_REGION_DEFINE(cvodeStepCompletedHandler, "CVodeStepCompleted");
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_START(measuredFunctionStartValues, cvodeStepCompletedHandler, "CVodeStepCompleted");
}
#endif
#ifdef RUNTIME_PROFILING
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_END(measuredFunctionStartValues, measuredFunctionEndValues, (*measureTimeFunctionsArray)[5], cvodeStepCompletedHandler);
}
#endif
// Perform state selection
bool state_selection = stateSelection();
if (state_selection)
_continuous_system->getContinuousStates(_z);
_zeroFound = false;
// Check if step was successful
if (check_flag(&_cv_rt, "CVode", 1))
{
_solverStatus = ISolver::SOLVERERROR;
break;
}
// A root was found
if ((_cv_rt == CV_ROOT_RETURN) && !isInterrupted())
{
// CVode is setting _tCurrent to the time where the first event occurred
double _abs = fabs(_tLastEvent - _tCurrent);
_zeroFound = true;
if ((_abs < 1e-3) && _event_n == 0)
{
_tLastEvent = _tCurrent;
_event_n++;
}
else if ((_abs < 1e-3) && (_event_n >= 1 && _event_n < 500))
{
_event_n++;
}
else if ((_abs >= 1e-3))
{
//restart event counter
_tLastEvent = _tCurrent;
_event_n = 0;
}
else
throw ModelicaSimulationError(EVENT_HANDLING,"Number of events exceeded in time interval " + to_string(_abs) + " at time " + to_string(_tCurrent));
// CVode has interpolated the states at time 'tCurrent'
_time_system->setTime(_tCurrent);
// To get steep steps in the result file, two value points (P1 and P2) must be added
//
// Y | (P2) X...........
// | :
// | :
// |........X (P1)
// |---------------------------------->
// | ^ t
// _tCurrent
// Write the values of (P1)
if (writeEventOutput)
{
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
writeToFile(0, _tCurrent, _h);
}
_idid = CVodeGetRootInfo(_cvodeMem, _zeroSign);
for (int i = 0; i < _dimZeroFunc; i++)
_events[i] = bool(_zeroSign[i]);
if (_mixed_system->handleSystemEvents(_events))
{
// State variables were reinitialized, thus we have to give these values to the cvode-solver
// Take care about the memory regions, _z is the same like _CV_y
_continuous_system->getContinuousStates(_z);
}
}
if ((_zeroFound || state_selection)&& !isInterrupted())
{
// Write the values of (P2)
if (writeEventOutput)
{
// If we want to write the event-results, we should evaluate the whole system again
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
writeToFile(0, _tCurrent, _h);
}
_idid = CVodeReInit(_cvodeMem, _tCurrent, _CV_y);
if (_idid < 0)
throw ModelicaSimulationError(SOLVER,"CVode::ReInit()");
// Der Eventzeitpunkt kann auf der Endzeit liegen (Time-Events). In diesem Fall wird der Solver beendet, da CVode sonst eine interne Warnung schmeißt
if (_tCurrent == _tEnd)
_cv_rt = CV_TSTOP_RETURN;
if(_continuous_system->stepCompleted(_tCurrent))
_solverStatus = DONE;
}
// Zähler für die Anzahl der ausgegebenen Schritte erhöhen
++_outStps;
_tLastSuccess = _tCurrent;
if (_cv_rt == CV_TSTOP_RETURN)
{
_time_system->setTime(_tEnd);
//Solver has finished calculation - calculate the final values
_continuous_system->setContinuousStates(NV_DATA_S(_CV_y));
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
if(writeOutput)
writeToFile(0, _tEnd, _h);
_accStps += _locStps;
_solverStatus = DONE;
}
}
}
/**
@param fileName the file to open
@param continueOnErrors whether to continue or immediately bail upon an error
@param noFileNotFoundMessage whether or not to log if we find a
file (we normally want to but for robot param files that'd be too
annoying since we test for a lot of files)
@param errorBuffer buffer to put errors into if not NULL. Only the
first error is saved, and as soon as this function is called it
immediately empties the errorBuffer
@param errorBufferLen the length of @a errorBuffer
*/
AREXPORT bool ArFileParser::parseFile(const char *fileName,
bool continueOnErrors,
bool noFileNotFoundMessage,
char *errorBuffer,
size_t errorBufferLen)
{
myInterruptMutex.lock();
myIsInterrupted = false;
myInterruptMutex.unlock();
FILE *file = NULL;
char line[10000];
bool ret = true;
if (errorBuffer)
errorBuffer[0] = '\0';
std::string realFileName;
if (fileName[0] == '/' || fileName[0] == '\\')
{
realFileName = fileName;
}
else
{
realFileName = myBaseDir;
realFileName += fileName;
}
ArLog::log(ArLog::Verbose, "Opening file %s from fileName given %s and base directory %s", realFileName.c_str(), fileName, myBaseDir.c_str());
//char *buf = new char[4096];
if ((file = ArUtil::fopen(realFileName.c_str(), "r")) == NULL)
{
if (errorBuffer != NULL)
snprintf(errorBuffer, errorBufferLen, "cannot open file %s", fileName);
if (!noFileNotFoundMessage)
ArLog::log(ArLog::Terse, "ArFileParser::parseFile: Could not open file %s to parse file.", realFileName.c_str());
return false;
}
/**
if( setvbuf( file, buf, _IOFBF, sizeof( buf ) ) != 0 )
printf( "Incorrect type or size of buffer for file\n" );
//else
// printf( "'file' now has a buffer of 1024 bytes\n" );
**/
resetCounters();
// read until the end of the file
while (!isInterrupted() &&
(fgets(line, sizeof(line), file) != NULL))
{
if (!parseLine(line, errorBuffer, errorBufferLen))
{
ArLog::log(ArLog::Terse, "## Last error on line %d of file '%s'",
myLineNumber, realFileName.c_str());
ret = false;
if (!continueOnErrors)
break;
}
}
fclose(file);
return ret;
}
LDL_int LDL_numeric /* returns n if successful, k if D (k,k) is zero */
(
LDL_int n, /* A and L are n-by-n, where n >= 0 */
LDL_int Ap [ ], /* input of size n+1, not modified */
LDL_int Ai [ ], /* input of size nz=Ap[n], not modified */
scs_float Ax [ ], /* input of size nz=Ap[n], not modified */
LDL_int Lp [ ], /* input of size n+1, not modified */
LDL_int Parent [ ], /* input of size n, not modified */
LDL_int Lnz [ ], /* output of size n, not defn. on input */
LDL_int Li [ ], /* output of size lnz=Lp[n], not defined on input */
scs_float Lx [ ], /* output of size lnz=Lp[n], not defined on input */
scs_float D [ ], /* output of size n, not defined on input */
scs_float Y [ ], /* workspace of size n, not defn. on input or output */
LDL_int Pattern [ ],/* workspace of size n, not defn. on input or output */
LDL_int Flag [ ], /* workspace of size n, not defn. on input or output */
LDL_int P [ ], /* optional input of size n */
LDL_int Pinv [ ] /* optional input of size n */
)
{
scs_float yi, l_ki ;
LDL_int i, k, p, kk, p2, len, top ;
for (k = 0 ; k < n ; k++)
{
if(isInterrupted()) {
scs_printf("interrupt detected in factorization\n");
return -1;
}
/* compute nonzero Pattern of kth row of L, in topological order */
Y [k] = 0.0 ; /* Y(0:k) is now all zero */
top = n ; /* stack for pattern is empty */
Flag [k] = k ; /* mark node k as visited */
Lnz [k] = 0 ; /* count of nonzeros in column k of L */
kk = (P) ? (P [k]) : (k) ; /* kth original, or permuted, column */
p2 = Ap [kk+1] ;
for (p = Ap [kk] ; p < p2 ; p++)
{
i = (Pinv) ? (Pinv [Ai [p]]) : (Ai [p]) ; /* get A(i,k) */
if (i <= k)
{
Y [i] += Ax [p] ; /* scatter A(i,k) into Y (sum duplicates) */
for (len = 0 ; Flag [i] != k ; i = Parent [i])
{
Pattern [len++] = i ; /* L(k,i) is nonzero */
Flag [i] = k ; /* mark i as visited */
}
while (len > 0) Pattern [--top] = Pattern [--len] ;
}
}
/* compute numerical values kth row of L (a sparse triangular solve) */
D [k] = Y [k] ; /* get D(k,k) and clear Y(k) */
Y [k] = 0.0 ;
for ( ; top < n ; top++)
{
i = Pattern [top] ; /* Pattern [top:n-1] is pattern of L(:,k) */
yi = Y [i] ; /* get and clear Y(i) */
Y [i] = 0.0 ;
p2 = Lp [i] + Lnz [i] ;
for (p = Lp [i] ; p < p2 ; p++)
{
Y [Li [p]] -= Lx [p] * yi ;
}
l_ki = yi / D [i] ; /* the nonzero entry L(k,i) */
D [k] -= l_ki * yi ;
Li [p] = k ; /* store L(k,i) in column form of L */
Lx [p] = l_ki ;
Lnz [i]++ ; /* increment count of nonzeros in col i */
}
if (D [k] == 0.0) return (k) ; /* failure, D(k,k) is zero */
}
return (n) ; /* success, diagonal of D is all nonzero */
}
