/*! Right now we define the distance as the max distance between any two
variables. Alternatively, the mean distance between all variables could
also be used. For position states we could also look at the distance in
actual position.
*/
double
VariableSet::distance(const VariableSet *s) const
{
if (getNumVariables() != s->getNumVariables()) return -1;
double distance = 0;
for (int i=0; i<getNumVariables(); i++) {
double altD;
double d = readVariable(i) - s->readVariable(i);
if ( getVariable(i)->isCircular() ) {
//if the variable is circular, the shortest distance might be around the end
//going one direction or the other
altD = fabs( readVariable(i) - getVariable(i)->mMinVal ) +
fabs( s->readVariable(i) - getVariable(i)->mMaxVal );
d = std::min(d, altD);
altD = fabs( s->readVariable(i) - getVariable(i)->mMinVal ) +
fabs( readVariable(i) - getVariable(i)->mMaxVal );
d = std::min(d, altD);
}
d = fabs(d) / getVariable(i)->getRange();
distance = std::max(d, distance);
}
return distance;
}
void
LinearSolver::updateLinearProgram() {
if (_parametersDirty) {
LOG_DEBUG(linearsolverlog) << "initializing solver" << std::endl;
if (_parameters.isSet())
_solver->initialize(
getNumVariables(),
_parameters->getDefaultVariableType(),
_parameters->getSpecialVariableTypes());
else
_solver->initialize(
getNumVariables(),
Continuous);
_parametersDirty = false;
}
if (_objectiveDirty) {
LOG_DEBUG(linearsolverlog) << "(re)setting objective" << std::endl;
_solver->setObjective(*_objective);
_objectiveDirty = false;
}
if (_linearConstraintsDirty) {
LOG_DEBUG(linearsolverlog) << "(re)setting linear constraints" << std::endl;
_solver->setConstraints(*_linearConstraints);
_linearConstraintsDirty = false;
}
if (_pinnedChanged) {
LOG_DEBUG(linearsolverlog) << "(un)pinning variables" << std::endl;
unsigned int varNum;
double value;
foreach (boost::tie(varNum, value), _pinned)
_solver->pinVariable(varNum, value);
foreach (varNum, _unpinned)
_solver->unpinVariable(varNum);
_unpinned.clear();
_pinnedChanged = false;
}
}
bool
VariableSet::readFromArray(std::vector<double> array)
{
if(getNumVariables() != (int) array.size())
{
DBGA("size does not match" << getNumVariables() << " " << array.size());
return false;
}
for(int i = 0; i < getNumVariables(); i ++)
{
mVariables[i]->setValue(array[i]);
}
return true;
}
/*
** updateData
**
** UNSAFE: Use updateData(std::vector<std::vector<float>> data)
**
** Data should come in the form[nVariables][nDataPoints]
**
** Loads new data onto the ScopePlot buffer. Must have the same number
** of variables initialized with setup. plot() may be called to display the
** updated buffer.
*/
void ofxScopePlot::updateData(float ** data, int nPoints) {
std::vector< std::vector<float> > vec_data;
vec_data.resize(getNumVariables());
for (int i=0; i<getNumVariables(); i++) {
vec_data.at(i).resize(nPoints);
for (int j=0; j<nPoints; j++) {
vec_data.at(i).at(j) = data[i][j];
}
//vec_data.at(i).assign(data[i], data[i] + nPoints
}
updateData(vec_data);
}
const SearchVariable* HandObjectState::getVariable(int i) const
{
if ( i < mPosture->getNumVariables() ) return mPosture->getVariable(i);
else if ( i < getNumVariables() ) return mPosition->getVariable( i - mPosture->getNumVariables() );
else assert(0);
return NULL;
}
bool
VariableSet::readFromFile(FILE *fp)
{
//read the type first and check against my own
int type;
fscanf(fp,"%d",&type);
if (type != getType()) {
fprintf(stderr,"Wrong type %d in state file (%d expected)\n",type,getType());
return false;
}
char line[10000];
int offset;
float v;
if (!fgets(line, 10000, fp)) {
fprintf(stderr,"Failed to read data from file!\n");
return false;
}
// should really learn how to use streams instead of these hacks...
offset = 0;
//read all variables
for (int i=0; i<getNumVariables(); i++) {
if ( line[offset]=='\0' ) {
fprintf(stderr,"Line to short to read all state variables\n");
return false;
}
while( isspace(*(line+offset)) ) offset++;
sscanf(line+offset,"%f",&v);
mVariables[i]->setValue(v);
while( !isspace(*(line+offset)) ) offset++;
}
return true;
}
void
LinearSolver::updateLinearProgram() {
if (_parametersDirty) {
LOG_DEBUG(linearsolverlog) << "initializing solver" << std::endl;
_solver->initialize(
getNumVariables(),
(_parameters ? _parameters->getVariableType() : Continuous));
_parametersDirty = false;
}
if (_objectiveDirty) {
LOG_DEBUG(linearsolverlog) << "(re)setting objective" << std::endl;
_solver->setObjective(*_objective);
_objectiveDirty = false;
}
if (_linearConstraintsDirty) {
LOG_DEBUG(linearsolverlog) << "(re)setting linear constraints" << std::endl;
_solver->setConstraints(*_linearConstraints);
_linearConstraintsDirty = false;
}
}
/*
** plot
** Plots the data in the buffer
*/
void ofxScopePlot::plot() {
//float xPlotScale = ofGetWindowSize().x / _pointsPerWin * ofGetWindowSize().x / (_max.x - _min.x);
float xPlotScale = (_max.x - _min.x) / (_pointsPerWin - 1);// * (_max.x - _min.x) / ofGetWindowSize().x;
float yPlotScale = (_max.y - _min.y) / ofGetWindowSize().y;
float yPlotOffset = ((_max.y - _min.y) / 2.);
//printf("xPlotScale: %f, yPlotScale: %f, yPlotOffset: %f\n", xPlotScale, yPlotScale, yPlotOffset);
//ofScale(1., 1., 1.);
// Background
ofEnableAlphaBlending();
ofSetColor(_backgroundColor);
ofRect(ofRectangle(_min, _max));
// Scope zero line
ofSetColor(_zeroLineColor);
ofLine(_min.x, _min.y + (_max.y - _min.y)/2, _max.x, _min.y + (_max.y - _min.y)/2);
ofDisableAlphaBlending();
for (int i=0; i<getNumVariables(); i++) {
ofSetColor(_variableColors.at(i));
ofSetLineWidth(_plotLineWidth);
for (int j=1; j<_pointsPerWin; j++) {
ofPoint p1 = ofPoint(_max.x-((float)(j-1)*xPlotScale),
_max.y-(yPlotScale*((_buffer.at(i).at(j-1) * _yScale + _yOffset)) + yPlotOffset));
ofPoint p2 = ofPoint(_max.x-((float)(j)*xPlotScale),
_max.y-(yPlotScale*((_buffer.at(i).at(j) * _yScale + _yOffset)) + yPlotOffset));
ofLine(p1, p2);
//printf("[%i, %i]: ofLine([%.1f, %.1f], [%.1f, %.1f]): buffer[%.1f],[%.1f]\n", i, j, p1.x, p1.y, p2.x, p2.y, _buffer[i][j-1], _buffer[i][j]);
//ofLine(_max.x-(float)(j-1), (_buffer[i][j-1] * _yScale) + yPlotOffset, _max.x-(float)j, (_buffer[i][j] * _yScale) + yPlotOffset);
//printf("[%i, %i]: ofLine(%f, %f, %f, %f)\n", i, j, _max.x-(float)(j-1), (_buffer[i][j-1] * _yScale) + yPlotOffset, _max.x-(float)j, (_buffer[i][j] * _yScale) + yPlotOffset);
}
}
}
/*
** setVariableColors
** Sets the plot colors of the variables.
** Must be called after setup with the same number of variables initialized.
*/
void ofxScopePlot::setVariableColors(std::vector<ofColor> colors) {
if (colors.size() != getNumVariables()) {
fprintf(stderr, "ERROR: colors.size() != getNumVariables()");
} else {
_variableColors = colors;
}
}
void DataTable::recordGridPoint(const DataPoint &sample)
{
for (unsigned int i = 0; i < getNumVariables(); i++)
{
grid.at(i).insert(sample.getX().at(i));
}
}
void DataTable::initDataStructures()
{
for (unsigned int i = 0; i < getNumVariables(); i++)
{
grid.push_back(std::set<double>());
}
}
void
VariableSet::writeToFile(FILE *fp) const
{
fprintf(fp,"%d ",getType());
for (int i=0; i<getNumVariables(); i++)
fprintf(fp,"%f ",mVariables[i]->getValue() );
fprintf(fp,"\n");
}
void
QuadraticSolver::updateQuadraticProgram() {
if (_parameters.isSet())
_solver->initialize(
getNumVariables(),
_parameters->getDefaultVariableType(),
_parameters->getSpecialVariableTypes());
else
_solver->initialize(
getNumVariables(),
Continuous);
_solver->setObjective(*_objective);
_solver->setConstraints(*_linearConstraints);
}
void
VariableSet::print() const
{
fprintf(stderr,"\n");
fprintf(stderr,"Type: %d\n",getType());
for (int i=0; i<getNumVariables(); i++) {
fprintf(stderr,"%s = %.2f; ",mVariables[i]->getName().latin1(),mVariables[i]->getValue());
}
fprintf(stderr,"\n");
}
void
QuadraticSolver::updateQuadraticProgram() {
_solver->initialize(
getNumVariables(),
(_parameters ? _parameters->getVariableType() : Continuous));
_solver->setObjective(*_objective);
_solver->setConstraints(*_linearConstraints);
}
/*
** updateData
**
** Data should come in the form[nVariables] with one data point/variable
**
** Loads new data onto the ScopePlot buffer. Must have the same number
** of variables initialized with setup. plot() may be called to display the
** updated buffer.
*/
void ofxScopePlot::updateData(std::vector<float> data) {
#ifdef DEBUG_PRINT
printf("ofxScopePlot::updateData\n");
#endif
if (data.size() != getNumVariables()) {
fprintf(stderr, "ERROR: data.size() != getNumVariables()");
} else {
for (int i=0; i<data.size(); i++) {
// Insert the data elements into the beginning of _buffer
_buffer.at(i).insert(_buffer.at(i).begin(), data.at(i));
// Erase elements at end of _buffer
_buffer.at(i).erase(_buffer.at(i).begin() + _pointsPerWin, _buffer.at(i).end());
}
}
}
DenseVector ConstraintBSpline::centralDifference(const DenseVector &x) const
{
DenseVector dx = DenseVector::Zero(nnzJacobian);
double h = 1e-6; // perturbation step size
int k = 0;
for (unsigned int i = 0; i < getNumVariables(); i++)
{
double hForward = 0.5*h;
DenseVector xForward(x);
if (xForward(i) + hForward > variables.at(i)->getUpperBound())
{
hForward = 0;
}
else
{
xForward(i) = xForward(i) + hForward;
}
double hBackward = 0.5*h;
DenseVector xBackward(x);
if (xBackward(i) - hBackward < variables.at(i)->getLowerBound())
{
hBackward = 0;
}
else
{
xBackward(i) = xBackward(i) - hBackward;
}
DenseVector yForward = eval(xForward);
DenseVector yBackward = eval(xBackward);
for (unsigned int j = 0; j < numConstraints; ++j)
{
dx(k++) = (yForward(j) - yBackward(j))/(hBackward + hForward);
}
}
return dx;
}
