static float noise2d_get(Noise2DContext* const ctx, const float x, const float y)
{
Vec2 p = { x, y };
// Cheap trick since inputs will always be [0, MAX_INT]
const int x0 = (int)x;
const int y0 = (int)y;
const int x1 = x0 + 1;
const int y1 = y0 + 1;
const float x0f = (float)x0;
const float y0f = (float)y0;
const Vec2 grad0 = get_gradient(ctx, x0, y0);
const Vec2 grad1 = get_gradient(ctx, x1, y0);
const Vec2 grad2 = get_gradient(ctx, x0, y1);
const Vec2 grad3 = get_gradient(ctx, x1, y1);
const Vec2 origin0 = { x0f, y0f };
const Vec2 origin1 = { x0f + 1.0f, y0f };
const Vec2 origin2 = { x0f, y0f + 1.0f };
const Vec2 origin3 = { x0f + 1.0f, y0f + 1.0f };
float v0 = gradient(origin0, grad0, p);
float v1 = gradient(origin1, grad1, p);
float v2 = gradient(origin2, grad2, p);
float v3 = gradient(origin3, grad3, p);
float fx = smooth(x - origin0.x);
float vx0 = lerp(v0, v1, fx);
float vx1 = lerp(v2, v3, fx);
float fy = smooth(y - origin0.y);
return lerp (vx0, vx1, fy);
}
void Noise3D::get_gradients(Vec3f *origins, Vec3f *grads,
float x, float y, float z) const
{
float x0f = std::floor(x);
float y0f = std::floor(y);
float z0f = std::floor(z);
int x0 = x0f;
int y0 = y0f;
int z0 = z0f;
int x1 = x0 + 1;
int y1 = y0 + 1;
int z1 = z0 + 1;
grads[0] = get_gradient(x0, y0, z0);
grads[1] = get_gradient(x0, y0, z1);
grads[2] = get_gradient(x0, y1, z0);
grads[3] = get_gradient(x0, y1, z1);
grads[4] = get_gradient(x1, y0, z0);
grads[5] = get_gradient(x1, y0, z1);
grads[6] = get_gradient(x1, y1, z0);
grads[7] = get_gradient(x1, y1, z1);
origins[0] = {x0f + 0.0f, y0f + 0.0f, z0f + 0.0f};
origins[1] = {x0f + 0.0f, y0f + 0.0f, z0f + 1.0f};
origins[2] = {x0f + 0.0f, y0f + 1.0f, z0f + 0.0f};
origins[3] = {x0f + 0.0f, y0f + 1.0f, z0f + 1.0f};
origins[4] = {x0f + 1.0f, y0f + 0.0f, z0f + 0.0f};
origins[5] = {x0f + 1.0f, y0f + 0.0f, z0f + 1.0f};
origins[6] = {x0f + 1.0f, y0f + 1.0f, z0f + 0.0f};
origins[7] = {x0f + 1.0f, y0f + 1.0f, z0f + 1.0f};
}
/*------------------------------------------------------- not finish
* Procedure to interpolate function value and transfer function
* value for texel colors and alpha values.
*/
void my_tex3dGI(void)
{
float n[3]; //normal, gradient
int i, j, k,byteRatio;
float x, y, z;
for(i=0;i<length_x;i++){
x = (float) i/(length_x-1.0);
for(j=0;j<length_y;j++){
y = (float) j/(length_y-1.0);
for(k=0;k<length_z;k++){
z = (float) k/(length_z-1.0);
valToratio(x, y, z,&byteRatio);
get_gradient(n, x, y, z);
//Because type is unsigned char
//n=-1~1 -> n+1=0~2 -> (n[0]+1.0)*127.5=0~255
tex3dGI[TEXEL3(i, j, k)] = (n[0]+1.0)*127.5;
tex3dGI[TEXEL3(i, j, k)+1] = (n[1]+1.0)*127.5;
tex3dGI[TEXEL3(i, j, k)+2] = (n[2]+1.0)*127.5;
//Because type is unsigned char , range of byteRatio is 1~255.
tex3dGI[TEXEL3(i, j, k)+3] = byteRatio;
//printf("%f\t%f\t%f\t\t%d\n",n[0],n[1],n[2],byteRatio);
}
}
}
}
void create_normal_map(t_env *e, t_obj *obj)
{
int x;
int y;
double *grad;
y = -1;
grad = NULL;
obj->mat.texture.bump =
(t_rgb**)malloc(sizeof(t_rgb*) * obj->mat.texture.h);
obj->mat.texture.bump == NULL ? error(e, E_MALLOC, NULL, 1) : 0;
while (++y < obj->mat.texture.h)
{
x = -1;
obj->mat.texture.bump[y] =
(t_rgb*)malloc(sizeof(t_rgb) * obj->mat.texture.w);
obj->mat.texture.bump[y] == NULL ? error(e, E_MALLOC, NULL, 1) : 0;
while (++x < obj->mat.texture.w)
{
grad = get_gradient(obj->mat.texture.img, y, x, obj->mat.texture);
grad == NULL ? error(e, E_MALLOC, NULL, 1) : 0;
obj->mat.texture.bump[y][x] = compute_gradient(grad, obj);
ft_memdel((void**)&grad);
}
}
}
double MaxFlowSegmentation::get_totalprob(int index0)
{
double gradient = get_gradient(index0);
double histo = get_histoprob(index0);
//double cost = k_weight*exp( -(v_weight*log_like+(1-v_weight)*gradient));
double cost = pow(k_weight, -(v_weight*histo+(1-v_weight)*gradient) );
return cost;
}
int get_velocity(RASTER3D_Region * region, struct Gradient_info *gradient_info,
const double x, const double y, const double z,
double *vel_x, double *vel_y, double *vel_z)
{
if (gradient_info->compute_gradient)
return get_gradient(region, gradient_info, y, x, z, vel_x, vel_y, vel_z);
return interpolate_velocity(region, gradient_info->velocity_maps, y, x, z,
vel_x, vel_y, vel_z);
}
void Noise2D::get_gradients(Vec2f *origins, Vec2f *grads, float x, float y) const
{
float x0f = floorf(x);
float y0f = floorf(y);
int x0 = x0f;
int y0 = y0f;
int x1 = x0 + 1;
int y1 = y0 + 1;
grads[0] = get_gradient(x0, y0);
grads[1] = get_gradient(x1, y0);
grads[2] = get_gradient(x0, y1);
grads[3] = get_gradient(x1, y1);
origins[0] = {x0f + 0.0f, y0f + 0.0f};
origins[1] = {x0f + 1.0f, y0f + 0.0f};
origins[2] = {x0f + 0.0f, y0f + 1.0f};
origins[3] = {x0f + 1.0f, y0f + 1.0f};
}
// Classic Perlin noise, 3D version
float base(float x, float y)
{
x *= xscale; //replace with multiplication
y *= xscale;
//get grid point
int X = fast_floor(x);
int Y = fast_floor(y);
x = x - X;
y = y - Y;
int gi00 = get_gradient(X+0,Y+0);
int gi01 = get_gradient(X+0,Y+1);
int gi10 = get_gradient(X+1,Y+0);
int gi11 = get_gradient(X+1,Y+1);
// Calculate noise contributions from each of the eight corners
float n00= dot(grad+2*gi00, x, y);
float n10= dot(grad+2*gi10, x-1, y);
float n01= dot(grad+2*gi01, x, y-1);
float n11= dot(grad+2*gi11, x-1, y-1);
// Compute the fade curve value for each of x, y, z
#if 1
float u = fade(x);
float v = fade(y);
#else
float u = x;
float v = y;
#endif
// Interpolate along x the contributions from each of the corners
float nx00 = mix(n00, n10, u);
float nx10 = mix(n01, n11, u);
// Interpolate the four results along y
float nxy = mix(nx00, nx10, v);
if (nxy < -1 || nxy > 1 ) printf("Error: noise %f \n", nxy);
//return nxy / 0.707106781f; //-1 to 1
return nxy; //-1 to 1
}
int main(int argc, char **argv)
{
void
(*hook_print_SE)(const qnu*, const Lagr*, const nmpc&) = NULL;
void
(*hook_print_LG)(const qnu*, const Lagr*, const nmpc&, const float*) = NULL;
void
(*hook_print_SD)(const unsigned int*, const double*) = NULL;
void
(*hook_print_TR)(const unsigned int*, const double*) = NULL;
double
(*hook_exec_control_horiz)(qnu*, const nmpc&, robot*) = NULL;
char errnote[256];
unsigned int sd_loop = 0, k = 0, current_tgt_no = 0;
float tgtdist, grad_dot_grad = 0.;
robot vme;
double sd_loop_time, time_last_cmd_sent = 0, now;
nmpc C;
cl_opts clopts =
{ false };
parse_command_line(argc, argv, &vme, &clopts);
parse_input_file(C, vme.conffile());
print_greeting(C);
qnu* qu = (qnu*) calloc(C.N, sizeof(qnu));
Lagr* p = (Lagr*) calloc(C.N, sizeof(Lagr));
//cmd* cmd_horiz = (cmd*) calloc(C.C, sizeof(cmd));
float* grad = (float*) calloc(C.N + 1, sizeof(float));
float* last_grad = (float*) calloc(C.N + 1, sizeof(float));
double* time_to_tgt = (double*) calloc(C.ntgt, sizeof(float));
C.cur_tgt = C.tgt;
C.control_step = 0;
init_qu_and_p(qu, p, C);
tgtdist = C.tgttol + 1; // Just to get us into the waypoint loop.
C.horizon_loop = 0;
/*
* This next block of decisions sort out the hooks used to print output.
* Rather than include the ifs in each loop, I'm using function pointers
* which are set at runtime based on CL flags for verbosity.
*/
if (clopts.selec_verbose)
{
hook_print_SE = &print_pathnerr;
hook_print_LG = &print_LG;
hook_print_SD = &print_SD;
hook_print_TR = &print_TR;
}
else if (clopts.selec_verbose)
{
hook_print_SE = &empty_output_hook;
hook_print_LG = &empty_output_hook;
hook_print_SD = &empty_output_hook;
hook_print_TR = &empty_output_hook;
}
else
{
if (clopts.selec_state_and_error_SE)
hook_print_SE = &print_pathnerr;
else
hook_print_SE = &empty_output_hook;
if (clopts.selec_lagrange_grad_LG)
hook_print_LG = &print_LG;
else
hook_print_LG = &empty_output_hook;
if (clopts.selec_SD_converged_SD)
hook_print_SD = &print_SD;
else
hook_print_SD = &empty_output_hook;
if (clopts.selec_target_reached_TR)
hook_print_TR = &print_TR;
else
hook_print_TR = &empty_output_hook;
}
if (clopts.selec_sim)
hook_exec_control_horiz = &exec_control_horiz_dummy;
else
hook_exec_control_horiz = &exec_control_horiz_vme;
if (!clopts.selec_sim)
{
vme.tcp_connect();
vme.update_poshead(qu, C);
}
/*
* Enter the loop which will take us through all waypoints.
*/
while (current_tgt_no < C.ntgt)
{
time_to_tgt[current_tgt_no] = -wall_time();
C.cur_tgt = &C.tgt[current_tgt_no * 2];
tgtdist = C.tgttol + 1;
while (tgtdist > .1)
{
C.horizon_loop += 1;
sd_loop = 0;
sd_loop_time = -wall_time();
/*
* SD loop.
*/
while (1)
{
sd_loop += 1;
grad_dot_grad = 0.;
/*
* The core of the gradient decent is in the next few lines:
*/
tgtdist = predict_horizon(qu, p, C);
get_gradient(qu, p, C, grad);
for (k = 0; k < C.N; k++)
{
grad_dot_grad += grad[k] * last_grad[k];
}
/*
* Detect direction changes in the gradient by inspecting the
* product <grad, last_grad>. If it is positive, then the
* iterations are successfully stepping to the minimum, and we
* can accelerate by increasing dg. If we overshoot (and the
* product becomes negative), then backstep and drop dg to a
* safe value.
*/
if (grad_dot_grad > 0)
{
C.dg *= 2;
for (k = 0; k < C.N; ++k)
{
qu[k].Dth -= C.dg * grad[k];
}
}
else
{
C.dg = 0.1; // TODO: Adaptive.
for (k = 0; k < C.N; ++k)
{
qu[k].Dth += C.dg * grad[k];
}
}
swap_fptr(&grad, &last_grad);
if (last_grad[C.N] < .1)
break;
if (sd_loop >= MAX_SD_ITER)
{
sprintf(errnote, "Reached %d SD iterations. Stopping.",
sd_loop);
report_error(EXCEEDED_MAX_SD_ITER, errnote);
}
}
hook_print_SE(qu, p, C);
hook_print_LG(qu, p, C, grad);
sd_loop_time += wall_time();
hook_print_SD(&sd_loop, &sd_loop_time);
C.control_step += C.C;
hook_exec_control_horiz(qu, C, &vme);
for (k = 0; k < C.N - C.C - 1; ++k)
{
// cmd_horiz[k].v = qu[k].v;
// cmd_horiz[k].Dth = qu[k].Dth;
qu[k].v = qu[k + C.C].v;
qu[k].Dth = qu[k + C.C].Dth;
}
if (C.control_step > MAX_NMPC_ITER * C.C)
{
sprintf(errnote,
"Reached %d NMPC steps without reaching tgt. Stopping.",
MAX_NMPC_ITER);
report_error(TRAPPED_IN_NMPC_LOOP, errnote);
}
/*
* The last thing we do is get the new position and heading for
* the next SD calculation.
*/
// vme.update_poshead(qu);
}
time_to_tgt[current_tgt_no] += wall_time();
hook_print_TR(¤t_tgt_no, &time_to_tgt[current_tgt_no]);
++current_tgt_no;
}
return 0;
}
// Classic Perlin noise, 3D version
float base(float x, float y, float z)
{
/*
// Find unit grid cell containing point
int X = fast_floor(x);
int Y = fast_floor(y);
int Z = fast_floor(z);
// Get relative xyz coordinates of point within that cell
x = x - X;
y = y - Y;
z = z - Z;
// Wrap the integer cells at 255 (smaller integer period can be introduced here)
X = X & 255;
Y = Y & 255;
Z = Z & 255;
*/
x /= 8.0; //replace with multiplication
y /= 8.0;
z /= 4.0;
//get grid point
int X = fast_floor(x);
int Y = fast_floor(y);
int Z = fast_floor(z);
//get interpolation ratio
//if (z >= 32 || z < 0) printf("ERROR1 z = %f \n", z);
x = x - X;
y = y - Y;
z = z - Z;
//if (z >= 32 || z < 0) printf("ERROR2 z = %f \n", z);
//if (x<0 || y<0 || z<0) printf("x,y,z= %f %f %f \n", x,y,z);
//if (z >= 32) printf("x,y,z= %f %f %f X,Y,Z= %i %i %i \n", x+X,y+Y,z+Z,X,Y,Z);
// Calculate a set of eight hashed gradient indices
int gi000 = get_gradient(X+0,Y+0,Z+0);
int gi001 = get_gradient(X+0,Y+0,Z+1);
int gi010 = get_gradient(X+0,Y+1,Z+0);
int gi011 = get_gradient(X+0,Y+1,Z+1);
int gi100 = get_gradient(X+1,Y+0,Z+0);
int gi101 = get_gradient(X+1,Y+0,Z+1);
int gi110 = get_gradient(X+1,Y+1,Z+0);
int gi111 = get_gradient(X+1,Y+1,Z+1);
/*
int gi000 = perm[X+perm[Y+perm[Z]]] % 12;
int gi001 = perm[X+perm[Y+perm[Z+1]]] % 12;
int gi010 = perm[X+perm[Y+1+perm[Z]]] % 12;
int gi011 = perm[X+perm[Y+1+perm[Z+1]]] % 12;
int gi100 = perm[X+1+perm[Y+perm[Z]]] % 12;
int gi101 = perm[X+1+perm[Y+perm[Z+1]]] % 12;
int gi110 = perm[X+1+perm[Y+1+perm[Z]]] % 12;
int gi111 = perm[X+1+perm[Y+1+perm[Z+1]]] % 12;
*/
// The gradients of each corner are now:
// g000 = grad3[gi000];
// g001 = grad3[gi001];
// g010 = grad3[gi010];
// g011 = grad3[gi011];
// g100 = grad3[gi100];
// g101 = grad3[gi101];
// g110 = grad3[gi110];
// g111 = grad3[gi111];
// Calculate noise contributions from each of the eight corners
float n000= dot(_grad3[gi000], x, y, z);
float n100= dot(_grad3[gi100], x-1, y, z);
float n010= dot(_grad3[gi010], x, y-1, z);
float n110= dot(_grad3[gi110], x-1, y-1, z);
float n001= dot(_grad3[gi001], x, y, z-1);
float n101= dot(_grad3[gi101], x-1, y, z-1);
float n011= dot(_grad3[gi011], x, y-1, z-1);
float n111= dot(_grad3[gi111], x-1, y-1, z-1);
// Compute the fade curve value for each of x, y, z
#if 1
float u = fade(x);
float v = fade(y);
float w = fade(z);
#else
float u = x;
float v = y;
float w = z;
#endif
// Interpolate along x the contributions from each of the corners
float nx00 = mix(n000, n100, u);
float nx01 = mix(n001, n101, u);
float nx10 = mix(n010, n110, u);
float nx11 = mix(n011, n111, u);
// Interpolate the four results along y
float nxy0 = mix(nx00, nx10, v);
float nxy1 = mix(nx01, nx11, v);
// Interpolate the two last results along z
float nxyz = mix(nxy0, nxy1, w);
return nxyz * 0.707106781; //-1 to 1
}
