static ObjectLocation get_object_filename(SuifObject *obj) {
ObjectLocation loc;
if (!obj) {
return(loc);
}
while (obj && !is_kind_of<FileBlock>(obj)) {
obj = obj->get_parent();
}
if (!obj) return loc;
FileBlock *fb = to<FileBlock>(obj);
return(ObjectLocation(fb->get_source_file_name(),
0));
}
static ObjectLocation read_line_number(const SuifObject* stmt){
Annote* annote = (to<AnnotableObject>(stmt))->peek_annote("line");
// have the line # -- return
if ( annote != NULL ) {
IInteger line = to<IntegerBrick>(to<BrickAnnote>(annote)->
get_brick(0))->get_value();
String file = to<StringBrick>(to<BrickAnnote>(annote)->
get_brick(1))->get_value();
return(ObjectLocation(file, line));
}
ObjectLocation loc;
return(loc);
}
void KITECH_MonteCarloLocalizationComp::RandomizeAt (double x, double y, double radius)
{
particles.clear ();
particles.reserve (maxParticles);
for (int i=0; i < maxParticles; ++i) {
double r = radius * (rand() % 1000) / 1000.;
double th = DEG2RAD(rand() % 360);
VirtualRobot vr;
vr.pos.x = x + r*cos(th);
vr.pos.y = y + r*sin(th);
vr.pos.theta = th;
vr.prob = 1. / maxParticles;
particles.push_back (vr);
}
variance = ObjectLocation (radius*radius, radius*radius, DEG2RAD(360*360));
}
bool KITECH_MonteCarloLocalizationComp::Predict(ObjectLocation position)
{
ObjectLocation delta = diff(position, positionPrev);
double ang = DeltaRad (delta.Atan(), positionPrev.theta);
double sgn = (-M_PI/2 < ang && ang < M_PI/2) ? 1. : -1.;
double deltaForward = sgn*delta.Length ();
double deltaAngle = delta.theta;
if ((-epsilon < deltaForward && deltaForward < epsilon) &&
(-epsilon < deltaAngle && deltaAngle < epsilon) ) return false;
double stdevDistance = sqrt(varianceDistance);
double stdevDirection = sqrt(varianceDirection);
// 로봇의 이동량(전진량, 회전량) 만큼 각각의 파티클을 움직인다.
// 이 때, 이동량에 비례하는 오차(Gaussian random 으로 생성)가 추가된다.
for (int i = 0, n = particles.size(); i < n; ++i) {
VirtualRobot &vr = particles[i];
double th = vr.pos.theta + deltaAngle / 2.;
double dx = deltaForward * cos(th);
double dy = deltaForward * sin(th);
double dth = deltaAngle;
double vdx = dx * stdevDistance * GaussRand ();
double vdy = dy * stdevDistance * GaussRand ();
double vdth = dth * stdevDirection * GaussRand ();
vr.pos += ObjectLocation(dx + vdx, dy + vdy, dth + vdth);
}
StocasticPosition ();
positionPrev = position;
return false;
}
