void FactorGraph::makeCavity( size_t i, bool backup ) {
// fills all Factors that include var(i) with ones
std::map<size_t,Factor> newFacs;
bforeach( const dai::Neighbor &I, nbV(i) ) // for all neighboring factors I of i
newFacs[I] = Factor(factor(I).nodes());
setFactors( newFacs, backup );
}
void CFactorGraph::makeCavity(unsigned i, bool backup) {
// fills all Factors that include var(i) with ones
map<size_t, CFactor> newFacs;
foreach( const Neighbor &I, nbV(i) )
// for all neighboring factors I of i
newFacs [I] = CFactor(factor(I).vars(), 1.0);
setFactors( newFacs, backup );
}
GpsMover::GpsMover(double linear_distance_factor,
double linear_heading_factor,
double angular_distance_factor,
double angular_heading_factor) {
setFactors(linear_distance_factor,
linear_heading_factor,
angular_distance_factor,
angular_heading_factor);
}
void Noise::setToDefaults( void )
{
SingleAttribute::setToDefaults();
setNoiseModel( (PHOTON) );
setFactors( vgm::Vec4f(0.025f, 0.025f, 4.f, 1.f) );
setChannelsSeparated( (false) );
setRandomTextureScaleFactors( vgm::Vec2f(1.f, 1.f) );
setUseTextureLessRandom( (false) );
}
void EinspurDynamikRealtime::integrate(double dT, double /*x*/, double /*y*/, double /*alpha*/)
{
if (dT > 0.1)
dT = 0.1; //Hardlimit for big times! (We can't handle that.)
/*yNull.x = x;
yNull.y = y;
yNull.alpha = alpha;*/
setFactors();
if (num_intsteps < 1)
num_intsteps = 1;
double h = dT / num_intsteps;
//std::cerr << "h: " << h << "dT: " << dT << "num_intsteps: " << num_intsteps << std::endl;
double s = 0;
for (int i = 0; i < num_intsteps; ++i)
{
dYOne = dFunction(yNull);
EinspurDynamikRealtimeState k1 = dYOne * h;
EinspurDynamikRealtimeState k2 = dFunction(yNull + k1 * (1.0 / 4.0)) * h;
EinspurDynamikRealtimeState k3 = dFunction(yNull + k1 * (3.0 / 32.0) + k2 * (9.0 / 32.0)) * h;
EinspurDynamikRealtimeState k4 = dFunction(yNull + k1 * (1932.0 / 2197.0) + k2 * (-7200.0 / 2197.0) + k3 * (7296.0 / 2197.0)) * h;
EinspurDynamikRealtimeState k5 = dFunction(yNull + k1 * (439.0 / 216.0) + k2 * (-8.0) + k3 * (3680.0 / 513.0) + k4 * (-845.0 / 4104.0)) * h;
EinspurDynamikRealtimeState k6 = dFunction(yNull + k1 * (-8.0 / 27.0) + k2 * (2.0) + k3 * (-3544.0 / 2565.0) + k4 * (1859.0 / 4104.0) + k5 * (-11.0 / 40.0)) * h;
EinspurDynamikRealtimeState zOne = yNull + k1 * (25.0 / 216.0) + k3 * (1408.0 / 2565.0) + k4 * (2197.0 / 4104.0) - k5 * (1.0 / 5.0);
yOne = yNull + k1 * (16.0 / 135.0) + k3 * (6656.0 / 12825.0) + k4 * (28561.0 / 56430.0) - k5 * (9.0 / 50.0) + k6 * (2.0 / 55.0);
s += pow(errorcontrol * h / (2 * norm(yOne - zOne)), 0.25);
/*
if((stepcount % 10)==0) {
std::cerr << "Integration Step:" << std::endl;
std::cerr << "x: " << yNull.x << ", y: " << yNull.y << ", alpha: " << yNull.alpha << ", v: " << yNull.v << std::endl;
std::cerr << "x: " << yOne.x << ", y: " << yOne.y << ", alpha: " << yOne.alpha << ", v: " << yOne.v << std::endl;
}
*/
if (yOne.u < 0)
{
std::cerr << "!!!u negative: " << yOne.u << "!!!";
std::cerr << " Setting u to zero!!!" << std::endl;
yOne.u = 0.0;
}
yNull = yOne;
++stepcount;
}
num_intsteps = (int)ceil(dT / s);
//std::cerr << "s: " << s << ", h: " << h << ", num_intsteps: " << num_intsteps << std::endl;
//std::cerr << "v: " << yOne.v << ", omegav: " << (this->iag * this->i[gear+1] * yOne.v / wr) << ", u: " << yOne.u << std::endl;
}
euler1_simpl::euler1_simpl(int factor1, int factor2, int limit) {
setFactors(factor1, factor2);
setLimit(limit);
}
euler1_simpl::euler1_simpl() {
setFactors(3, 5);
setLimit(100);
}
