bool MotionControl::rollStart(Pose pose) {
static bool rotated = false;
if (floateq(pose.theta, M_PI, 20*M_PI/180))
rotated = true;
bool finished = rotated && floateq(pose.theta, 0, 20*M_PI/180);
if (finished) {
ctrl.angle = 0;
} else {
ctrl.angle = -1 / 10.0 * M_PI;
}
return !finished;
}
int gretl_iszero (int t1, int t2, const double *x)
{
double sum = 0.0;
int t;
for (t=t1; t<=t2; t++) {
if (!na(x[t])) {
sum += x[t] * x[t];
}
}
return floateq(sum, 0.0);
}
static int check(size_t* start, size_t* count)
{
int ok = 1;
Odom* odom = odom_create(RANK);
float* result = (float*)threeD;
float* expected = (float*)threeD;
odom_set(odom,start,count);
while(odom_more(odom)) {
size_t offset = odom_count(odom);
if(floateq(result[offset],expected[offset])) {
fprintf(stderr,"fail: result[%lu] = %f ; expected[%lu] = %f\n",
(long unsigned)offset,result[offset],(long unsigned)offset,expected[offset]);
ok=0;
}
odom_incr(odom);
}
odom_reclaim(odom);
return ok;
}
static int
check(size_t* start, size_t* count)
{
int ok = 1;
int index = 0;
Odom* odom = odom_create(RANK);
odom_set(odom,start,count);
float* result = (float*)target;
float* expected = (float*)target_data;
for(index=0;odom_more(odom);odom_incr(odom),index++) {
size_t offset = odom_count(odom);
if(!floateq(result[index],expected[offset])) {
fprintf(stderr,"fail: result[%lu] = %f ; expected[%lu] = %f\n",
index,result[index],offset,expected[offset]);
ok=0;
}
}
odom_reclaim(odom);
return ok;
}
static int
check(float* target, size_t* start, size_t* count)
{
int ok = 1;
Odom* odom = odom_create(RANK);
float* result = (float*)target;
float* expected = (float*)target_content;
odom_set(odom,start,count);
while(odom_more(odom)) {
size_t offset = odom_count(odom);
int eq = floateq(result[offset],expected[offset]);
if(eq == 0) {
fprintf(stderr,"fail: result[%lu] = %f ; expected[%lu] = %f\n",
offset,result[offset],offset,expected[offset]);
ok=0;
}
odom_incr(odom);
}
odom_reclaim(odom);
return ok;
}
// Main control loop to read current pose and update control values
// return false if no destination available (i.e. routing finished and need a new endpoint)
bool MotionControl::iterate(SLAM::RobotLocation myPose) {
//if (myPose.theta < 0) myPose.theta += 2*M_PI;
// you spin me right round
while (myPose.theta > M_PI)
myPose.theta -= 2*M_PI;
while (myPose.theta < -M_PI)
myPose.theta += 2*M_PI;
lastPose = myPose;
cout << " " << myPose.theta << endl;
cout << "ITERATE" << endl;
cout << midpoints.size() << endl;
if (midpoints.size() == 0) {
cout << "\tNo midpoints" << endl;
ctrl.speed = 0;
ctrl.angle = 0;
return false;
}
SLAM::Location midpdest = midpoints.front();
float dx = midpdest.x - myPose.x;
float dy = midpdest.y - myPose.y;
// TODO: subtract mod2pi somehow so that almost2pi-littleover0=2pi-almost2pi+littleover0 (?)
//if (myPose.theta<0.01)myPose.theta=2*M_PI;
float rho = sqrt(dx * dx + dy * dy);
float ata = atan2(dy, dx);
//if (ata < 0) ata += 2*M_PI;
float alpha = ata - myPose.theta;
alpha = fixangles(alpha);
// When new route is gotten, first rotate to orientation of first waypoint.
static const float eps = 20*M_PI/180;
if (routeStarting) {
if (floateq(fixangles(myPose.theta), fixangles(startPoint.theta), eps)) {
routeStarting = false;
return nextMidpoint();
} else {
std::cout << "Route starting" << std::endl;
ctrl.speed = 0;
float dt = startPoint.theta - myPose.theta;
dt = fixangles(dt);
ctrl.angle = k.a * dt;
return true;
}
}
ctrl.speed = k.p - k.iiris * fabsf(alpha);// * rho;
ctrl.angle = k.a * alpha;
cout<< "\tdx = " << dx << endl;
cout<< "\tdy = " << dy << endl;
cout<< "\tmy theta = " << myPose.theta << " " << r2d(myPose.theta) << endl;
cout<< "\tatan2 dydx = " << ata << " " << r2d(ata) << endl;
cout<< "\trho = " << rho << endl;
cout<< "\talpha = " << alpha << " " << r2d(alpha) << endl;
cout<< "\tv = " << ctrl.speed << endl;
cout<< "\tw = " << ctrl.angle << " " << r2d(ctrl.angle) << endl;
history.push_back(HistPoint{lastPose, Ctrl{ctrl.speed, ctrl.angle}, alpha, ata});
float enough = (midpoints.size() > 1) ? k.closeEnough : k.closeEnoughLast;
if (rho < enough) {
cout << "\tCLOSE ENOUGH!" << endl;
return nextMidpoint();
}
return true;
}
static int
trace_path(pathnode_t *path)
{
unsigned char type; /* type of reflection */
int hit;
int prev_interior;
double bsdf[3];
ri_vector_t outdir;
ri_ray_t ray;
ri_intersection_state_t next_state;
ri_material_t *material;
if (path->depth >= MAX_PATH_VERTICES) {
return 0;
}
material = path->state.geom->material;
if (!russian_roulette(material)) {
return 0;
}
type = sample_reflection_type(material);
prev_interior = path->interior;
if (path->interior && !floateq(material->ior, 1.0f)) {
/* ray exits from a transparent object */
path->interior = 0;
}
sample_outdir(&outdir, &type,
path->interior, material,
path->indir, path->state.Ng);
if (type == 'T' && prev_interior) {
/* ray enters into a transparent object */
path->interior = 1;
}
/* find next surface point where ray hits */
ri_vector_copy(&ray.org, path->state.P);
ri_vector_copy(&ray.dir, outdir);
ray.thread_num = 0;
hit = ri_raytrace(ri_render_get(), &ray, &next_state);
if (!hit) {
return 0;
}
brdf(bsdf,
type, &path->state,
path->indir, outdir, path->state.Ng);
path->G[0] *= bsdf[0];
path->G[1] *= bsdf[1];
path->G[2] *= bsdf[2];
path->depth++;
ri_vector_copy(&path->indir, outdir);
//ri_vector_neg(&path->indir);
ri_mem_copy(&path->state, &next_state, sizeof(ri_intersection_state_t));
return trace_path(path);
}
