GEBackground::GEBackground(int x, int y,std::string spriteloc) : GObject(x,y,0,0,true)
{
this->setSprite(spriteloc);
setW(getSprite()->w);
setH(getSprite()->h);
}
void Z80gb::setFlags(short z, short n, short h, short c) {
if(z >= 0) {
if(z)
setZF();
else
unsetZF();
}
if(n >= 0) {
if(n)
setN();
else
unsetN();
}
if(h >= 0) {
if(h)
setH();
else
unsetH();
}
if(c >= 0) {
if(c)
setCY();
else
unsetCY();
}
}
void Objets::setPlan(int _x, int _y, int _w, int _h) {
setX(_x);
setY(_y);
setW(_w);
setH(_h);
genererModelisation();
}
void Rect::grow(float amount) {
setX(getX() - amount);
setY(getY() - amount);
setW(getW() + amount * 2);
setH(getH() + amount * 2);
}
Ship::Ship(unsigned int w, unsigned int h, unsigned int hp, unsigned int score, unsigned int flag) {
setW(w);
setH(h);
setHp(hp);
setScore(score);
setFlag(flag);
return ;
}
Ship::Ship(void){
setW(0);
setH(0);
setHp(1);
setScore(0);
setFlag(0);
return ;
}
void Outils::ajouterBouton(Boutons *b) {
lesBoutons.push_back(b);
blit(b->getBouton(),lesOutils,0,0,b->getX(),b->getY(),b->getW(),b->getH());
setH(b->getY()+b->getH());
line(lesOutils,getW()-1,0,getW()-1,getH(),0);
line(lesOutils,getW(),getH(),0,getH(),0);
line(lesOutils,0,getH(),0,0,0);
}
MGAStarNode(int x, int y, const MGAStarNode& goal, double g = 0.0)
: m_X(x),
m_Y(y),
m_ParentX(x),
m_ParentY(y),
m_H(0.0),
m_G(g)
{
setH(heuristic(goal));
}
LawnMower::LawnMower()
{
setX(350);
setY(198);
setW(50);
setH(50);
setType("LawnMower");
setHealthImpact(25);
isCollided = false;
}
Hole::Hole()
{
setX(150);
setY(208);
setW(75);
setH(300);
setType("Hole");
setHealthImpact(100);
isCollided = false;
}
MadDog::MadDog()
{
setX(250);
setY(177);
setW(75);
setH(75);
setType("MadDog");
setHealthImpact(50);
isCollided = false;
}
void CSulGuiAlign::onViewResize( float w, float h )
{
// the align element doesn't have a size,.. so we need to create a size for it
// right now if there is a parent window we use that size, otherwise we use the size of
// the application window
// NOTE: this is not correct.. when rendering to a texture the highest window is not the
// application but the RTT texture
CSulGuiCanvas* p = dynamic_cast<CSulGuiCanvas*>(getParent(0));
setW( p?p->getW():w );
setH( p?p->getH():h );
update();
}
// ***************************************************************************
bool CDBViewDigit::parse (xmlNodePtr cur, CInterfaceGroup * parentGroup)
{
if(!CViewBase::parse(cur, parentGroup))
return false;
CViewRenderer &rVR = *CViewRenderer::getInstance();
// link to the db
CXMLAutoPtr ptr;
ptr = xmlGetProp (cur, (xmlChar*)"value");
if ( ptr )
_Number.link ( ptr );
else
{
nlinfo ("no value in %s", _Id.c_str());
return false;
}
// read options
ptr = xmlGetProp (cur, (xmlChar*)"numdigit");
if(ptr) fromString((const char*)ptr, _NumDigit);
clamp(_NumDigit, 1, 10);
ptr = xmlGetProp (cur, (xmlChar*)"wspace");
if(ptr) fromString((const char*)ptr, _WSpace);
ptr= (char*) xmlGetProp( cur, (xmlChar*)"color" );
_Color = CRGBA(255,255,255,255);
if (ptr)
_Color = convertColor (ptr);
// compute window size. Remove one space.
sint32 wDigit= rVR.getFigurTextureW();
sint32 hDigit= rVR.getFigurTextureH();
setW((wDigit+_WSpace)*_NumDigit - _WSpace);
setH(hDigit);
// some init
// For _NumDigit=2; set the divBase to 100, etc...
_DivBase= 1;
for(uint i= 0;i<(uint)_NumDigit;i++)
{
_DivBase*= 10;
}
// init cache.
_Cache= -1;
return true;
}
void SplitterWindow2::setMinSize(wxWindow* window, const wxSize& minSize) {
assert(m_splitMode != SplitMode_Unset);
assert(minSize.x >= 0 && minSize.y != 0);
wxSize splitterMinSize;
for (size_t i = 0; i < NumWindows; ++i) {
if (m_windows[i] == window)
m_minSizes[i] = minSize;
setH(splitterMinSize, h(splitterMinSize) + h(m_minSizes[i]));
setV(splitterMinSize, std::max(v(splitterMinSize), v(m_minSizes[i])));
}
SetMinClientSize(splitterMinSize);
}
CuriousCat::CuriousCat(QWidget *parent)
{
setX(128);
setY(450);
setW(192);
setH(192);
setType("CuriousCat");
//jumpSpeed = 100;
//gravity = 0;
height = 0;
speed = 200;
isClimbing = false;
isFalling = false;
isLanded = true;
counter = 0;
catMovie = new QMovie(":/cat.gif");
cat = new QLabel(parent);
cat->setMovie(catMovie);
catMovie->start();
cat->setGeometry(128,450, 192, 192);
cat->setScaledContents(true);
cat->show();
QLabel * mouthSensor = new QLabel(parent);
mouthSensor->setGeometry(mouthX, mouthY, sensorW, sensorH);
catSensors.push_back(mouthSensor);
QLabel * frontPawSensor = new QLabel(parent);
frontPawSensor->setGeometry(frontPawX,frontPawY, sensorW,sensorH);
catSensors.push_back(frontPawSensor);
QLabel * backPawSensor = new QLabel(parent);
backPawSensor->setGeometry(frontPawX - 75, frontPawY, sensorW, sensorH);
catSensors.push_back(backPawSensor);
}
void PataponPopup::setSize(int w, int h)
{
setW(w);
setH(h);
updateGeometry();
}
/*
* s e t u p A u x i l i a r y Q P
*/
returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new,
const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
)
{
int i;
int nV = getNV( );
int nC = getNC( );
returnValue returnvalue;
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
{
return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
}
status = QPS_PREPARINGAUXILIARYQP;
/* I) SETUP NEW QP MATRICES AND VECTORS: */
/* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
* that old optimal solution remains feasible for new QP data. */
/* Firstly, shift by -A'*x_opt and ... */
if ( nC > 0 )
{
if ( A_new == 0 )
return THROWERROR( RET_INVALID_ARGUMENTS );
for ( i=0; i<nC; ++i )
{
lbA[i] = -Ax_l[i];
ubA[i] = Ax_u[i];
}
/* Set constraint matrix as well as ... */
setA( A_new );
/* ... secondly, shift by +A_new'*x_opt. */
for ( i=0; i<nC; ++i )
{
lbA[i] += Ax[i];
ubA[i] += Ax[i];
}
/* update constraint products. */
for ( i=0; i<nC; ++i )
{
Ax_u[i] = ubA[i] - Ax[i];
Ax_l[i] = Ax[i] - lbA[i];
}
}
/* 2) Set new Hessian matrix, determine Hessian type and
* regularise new Hessian matrix if necessary. */
/* a) Setup new Hessian matrix and determine its type. */
if ( H_new != 0 )
{
setH( H_new );
hessianType = HST_UNKNOWN;
if ( determineHessianType( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* b) Regularise new Hessian if necessary. */
if ( ( hessianType == HST_ZERO ) ||
( hessianType == HST_SEMIDEF ) ||
( usingRegularisation( ) == BT_TRUE ) )
{
regVal = 0.0; /* reset previous regularisation */
if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
}
}
else
{
if ( H != 0 )
return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
}
/* 3) Setup QP gradient. */
if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
/* 1) Make a copy of current bounds/constraints ... */
Bounds oldBounds = bounds;
Constraints oldConstraints = constraints;
/* we're trying to find an active set with positive definite null
* space Hessian twice:
* - first for the current active set including all equalities
* - second after moving all inactive variables to a bound
* (depending on Options). This creates an empty null space and
* is guaranteed to succeed. Thus this loop will exit after n_try=1.
*/
int n_try;
for (n_try = 0; n_try < 2; ++n_try) {
if (n_try > 0) {
// the current active set leaves an indefinite null space Hessian
// move all inactive variables to a bound, creating an empty null space
for (int ii = 0; ii < nV; ++ii)
if (oldBounds.getStatus (ii) == ST_INACTIVE)
oldBounds.setStatus (ii, options.initialStatusBounds);
}
/* ... reset them ... */
bounds.init( nV );
constraints.init( nC );
/* ... and set them up afresh. */
if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* 2) Setup TQ factorisation. */
if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
// check for equalities that have become bounds ...
for (int ii = 0; ii < nC; ++ii) {
if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
oldConstraints.setStatus (ii, ST_UPPER);
else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
oldConstraints.setStatus (ii, ST_LOWER);
}
}
// ... and do the same also for the bounds!
for (int ii = 0; ii < nV; ++ii) {
if (oldBounds.getType(ii) == ST_EQUALITY
&& bounds.getType(ii) == ST_BOUNDED) {
if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
oldBounds.setStatus(ii, ST_UPPER);
else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
oldBounds.setStatus(ii, ST_LOWER);
}
}
/* 3) Setup old working sets afresh (updating TQ factorisation). */
if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* Factorise projected Hessian
* this now handles all special cases (no active bounds/constraints, no nullspace) */
returnvalue = computeProjectedCholesky( );
/* leave the loop if decomposition was successful, i.e. we have
* found an active set with positive definite null space Hessian */
if ( returnvalue == SUCCESSFUL_RETURN )
break;
}
/* adjust lb/ub if we changed the old active set in the second try
*/
if (n_try > 0) {
// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
for (int ii = 0; ii < nC; ++ii)
Ax_l[ii] = Ax_u[ii] = Ax[ii];
setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
}
status = QPS_AUXILIARYQPSOLVED;
return SUCCESSFUL_RETURN;
}
void Rect::setRect( RectBase rect ) {
setX(rect.x);
setY(rect.y);
setW(rect.width);
setH(rect.height);
}
void Objets::rotation(int p_x, int p_y, int p_w, int p_h, int angle) {
bool rota=true;
int b_x,b_y,b_w,b_h;
b_x=getX();
b_y=getY();
b_w=getW();
b_h=getH();
int a,b,c,d;
if ((angle == 90) || (angle==-90)) {
a=getX()+(getW()/2)-(getH()/2);
b=getY()+(getH()/2)-(getW()/2);
c=getH();
d=getW();
setX(a);
setY(b);
setW(c);
setH(d);
}
if (getX() < p_x)
setX(p_x);
if (getY() < p_y)
setY(p_y);
if (getX()+getW() > p_x+p_w)
setX(getX()-((getX()+getW())-(p_x+p_w)));
if (getY()+getH() > p_y+p_h)
setY(getY()-((getY()+getH())-(p_y+p_h)));
if ((getW() > p_w) || (getH() > p_h)) {
setX(b_x);
setY(b_y);
setW(b_w);
setH(b_h);
}
else {
if (angle==90) {
if (sens=="O")
sens="N";
else if (sens=="N")
sens="E";
else if (sens=="E")
sens="S";
else if (sens=="S")
sens="O";
}
else if (angle==-90) {
if (sens=="O")
sens="S";
else if (sens=="S")
sens="E";
else if (sens=="E")
sens="N";
else if (sens=="N")
sens="O";
}
else if (angle==180) {
if (sens=="O")
sens="E";
else if (sens=="N")
sens="S";
else if (sens=="E")
sens="O";
else if (sens=="S")
sens="N";
}
}
modelisation=create_bitmap(getW(),getH());
genererModelisation();
}
void threads::localization::update()
{
bool new_datum_available = false;
///// BEGIN LOCKED SECTION
{
// pop Datum from queue
std::lock_guard<std::mutex> lock(queue_mutex);
//printf("is queue empty? size = %d\n", data_queue.size());
if (!data_queue.empty())
{
new_datum_available = true;
current_datum = data_queue.top();
data_queue.pop();
//printf("popped datum from queue, size = %d\n", data_queue.size());
}
else
{
//printf("no datum available\n");
}
}
///// END LOCKED SECTION
//if there is a datum to process, continue, else return
if (!new_datum_available) return;
//printf("Processing a datum of type: %s", current_datum.type_string().c_str());
//std::cout << " value = " << current_datum.value() << std::endl;
// prediction step
// calculate dt using current time and datum time stamp
double dt = utility::time_tools::dt(t, current_datum.timestamp());
if (dt < 0)
{
printf("WARNING: localization timestep is negative, skipping sensor update\n");
return;
}
predict(dt);
t = current_datum.timestamp();
// convert value std::vector into a column matrix
std::vector<double> value = current_datum.value();
if (current_datum.type() == SENSOR_TYPE::GPS)
{
containers.heartbeat_gps = 1;
value.at(0) -= home_x; // use local frame
value.at(1) -= home_y;
// save gps information for use in gps velocity calculation
tR = utility::time_tools::dt(t0, t);
//printf("t = %f seconds\n", tR);
gps_data_t.push_back(utility::time_tools::dt(t0, current_datum.timestamp()));
gps_data_x.push_back(value.at(0));
gps_data_y.push_back(value.at(1));
// eliminate old gps data
for (int i = gps_data_t.size()-1; i > -1; i--)
{
if (tR - gps_data_t.at(i) > GPS_HISTORY_TIME_WINDOW)
{
//printf("gps datum age = %f seconds, deleting it...\n", tR - gps_data_t.at(i));
gps_data_t.erase(gps_data_t.begin() + i);
gps_data_x.erase(gps_data_x.begin() + i);
gps_data_y.erase(gps_data_y.begin() + i);
}
}
// if there are enough gps data remaining, calculate dx/dt and dy/dt
if (gps_data_t.size() >= GPS_HISTORY_REQUIRED_SIZE)
{
//printf("There are at least %d gps data available, calculating velocities...\n", GPS_HISTORY_REQUIRED_SIZE);
double n, s_t, s_x, s_y, s_tx, s_ty, s_tt, dx_dt, dy_dt;
n = (double)gps_data_t.size();
s_t = std::accumulate(gps_data_t.begin(), gps_data_t.end(), 0.0);
s_x = std::accumulate(gps_data_x.begin(), gps_data_x.end(), 0.0);
s_y = std::accumulate(gps_data_y.begin(), gps_data_y.end(), 0.0);
s_tt = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_t.begin(), 0.0);
s_tx = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_x.begin(), 0.0);
s_ty = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_y.begin(), 0.0);
dx_dt = (n*s_tx - s_t*s_x)/(n*s_tt - s_t*s_t);
dy_dt = (n*s_ty - s_t*s_y)/(n*s_tt - s_t*s_t);
//printf("dx_dt = %f m/s, dy_dt = %f m/s\n", dx_dt, dy_dt);
std::vector<double> velocities = {dx_dt, dy_dt};
Eigen::MatrixXd covariance(2, 2);
covariance = Eigen::MatrixXd::Identity(2, 2);
Datum datum(SENSOR_TYPE::GPS_VELOCITY, SENSOR_CATEGORY::LOCALIZATION, velocities, covariance);
new_sensor_update(datum); // put this gps_velocity datum into the queue
}
}
Eigen::Map<Eigen::MatrixXd> z_(value.data(), value.size(), 1);
z = z_;
R = current_datum.covariance();
setH();
K = P*H.transpose()*(H*P*H.transpose() + R).inverse();
dz = z - H*state;
if (current_datum.type() == SENSOR_TYPE::COMPASS)
{
dz(0, 0) = utility::angle_tools::minimum_difference(dz(0, 0)); // use true angular difference, not algebraic difference
}
S = H*P*H.transpose();
if (S.determinant() < pow(10.0, -12.0))
{
printf("WARNING: innovation covariance is singular for sensor type: %s\n", current_datum.type_string().c_str());
std::cout << "z = " << z << std::endl;
std::cout << "H = " << H << std::endl;
std::cout << "R = " << R << std::endl;
std::cout << "P = " << P << std::endl;
std::cout << "K = " << K << std::endl;
std::cout << "S = " << S << std::endl;
std::cout << "det(S) = " << S.determinant() << " vs. " << pow(10.0, -6.0) << std::endl;
return;
}
double d = sqrt((dz.transpose()*S.inverse()*dz)(0, 0));
//printf("Mahalonobis distance = %f\n", d);
state += K*dz;
P = (StateSizedSquareMatrix::Identity() - K*H)*P;
//std::cout << "Updated state = " << state.transpose() << std::endl;
updateKB();
}
//--------------------------------------------------------------
Rect::Rect() {
setX(0);
setY(0);
setW(0);
setH(0);
}
Ship &Ship::operator=(Ship const & rhs){
setW(rhs.getW());
setH(rhs.getH());
return *this;
}
void Ship::move(unsigned int w, unsigned int h) {
setW(w);
setH(h);
return ;
}
covafill<scalartype_>::covafill(matrixtype coordinates_,
vectortype observations_,
vectortype h_,
int p_) : coordinates(coordinates_), observations(observations_), p(p_), Hinv(coordinates.cols(),coordinates.cols()), detHinv(0), dim(coordinates.cols()), nobs(coordinates.rows()) {
setH(h_);
}