void AimerDemo::on_render()
{
glClear(GL_COLOR_BUFFER_BIT);
// Draw aimer preprocessed data
glUseProgram(m_aimer_program);
mat3f aimer_matrix = mat3f::eye()
.scale(arc_radius(), arc_radius())
.rotate(tangent_angle())
.translate(origin())
.premul(m_camera.matrix());
glEnableVertexAttribArray(m_aimer_position_attrib);
// Draw grid
glUniformMatrix3fv(m_aimer_matrix_uniform, aimer_matrix * m_aimer.m_grid_box.matrix_to());
glUniform3f(m_aimer_color_uniform, 0.8f, 0.8f, 0.8f);
glBindBuffer(GL_ARRAY_BUFFER, m_grid_vertices);
glVertexAttribPointer(m_aimer_position_attrib, 2, GL_FLOAT, GL_FALSE, 0, 0);
glDrawArrays(GL_LINES, 0, 4 * (ClothoidAimer::GRID_SIZE + 1));
// Draw samples
glUniformMatrix3fv(m_aimer_matrix_uniform, aimer_matrix);
glUniform3f(m_cloth_color_uniform, 0.5f, 0.5f, 0.8f);
glBindBuffer(GL_ARRAY_BUFFER, m_sample_vertices);
glVertexAttribPointer(m_cloth_position_attrib, 2, GL_FLOAT, GL_FALSE, 0, 0);
glDrawArrays(GL_LINES, 0, m_aimer.m_samples.size() * 4);
glDisableVertexAttribArray(m_aimer_position_attrib);
// Draw clothoid
glUseProgram(m_cloth_program);
glUniformMatrix3fv(m_cloth_matrix_uniform, m_camera.matrix());
glEnableVertexAttribArray(m_cloth_position_attrib);
if (m_cloth_ready)
{
glUniform4f(m_cloth_color_uniform, 1, 0, 0, 1);
glBindBuffer(GL_ARRAY_BUFFER, m_cloth_vertices);
glVertexAttribPointer(m_cloth_position_attrib, 2, GL_FLOAT, GL_FALSE, 0, 0);
glDrawArrays(GL_LINE_STRIP, 0, CLOTHOID_VERTEX_COUNT);
}
glDisableVertexAttribArray(m_cloth_position_attrib);
ConstrainedClothoidDemo::on_render();
}
void AimerDemo::aim()
{
real theta0 = tangent_angle();
real kappa0 = rl(1) / rl(arc_radius());
m_aim_result = m_aimer.aim(origin(), theta0, kappa0, target());
real a = m_aim_result.a;
real s = m_aim_result.s;
m_a = float(a); m_s = float(s);
m_aim_eval = origin() + arc_radius() * Fresnel::eval(theta0, 1, a, s); // TODO
std::vector<vec2f> cloth_vertices;
for (int i = 0; i < CLOTHOID_VERTEX_COUNT; ++i)
{
real si = i * s / (CLOTHOID_VERTEX_COUNT - 1);
cloth_vertices.push_back(
origin() + arc_radius() * Fresnel::eval(theta0, 1, a, si)
);
}
glBindBuffer(GL_ARRAY_BUFFER, m_cloth_vertices);
glBufferData(GL_ARRAY_BUFFER, cloth_vertices);
m_cloth_ready = true;
}
void nv_shapecontext_feature(nv_shapecontext_t *sctx,
const nv_matrix_t *img,
float r
)
{
int m, row, col, pc, i, l;
nv_matrix_t *edge = nv_matrix3d_alloc(1, img->rows, img->cols);
nv_matrix_t *points = nv_matrix_alloc(2, img->m);
int *rand_idx = (int *)nv_malloc(sizeof(int) * img->m);
float u_x, u_y, p_x, p_y, r_e;
int pn;
// 細線化
nv_matrix_zero(points);
nv_shapecontext_edge_image(edge, img);
pc = 0;
u_x = 0.0f;
u_y = 0.0f;
for (row = 0; row < edge->rows; ++row) {
for (col = 0; col < edge->cols; ++col) {
if (NV_MAT3D_V(edge, row, col, 0) > 50.0f) {
NV_MAT_V(points, pc, 0) = (float)row;
NV_MAT_V(points, pc, 1) = (float)col;
++pc;
u_y += (float)row;
u_x += (float)col;
}
}
}
u_x /= pc;
u_y /= pc;
// 指定数の特徴にする（ランダム）
pn = NV_MIN(pc, sctx->sctx->list);
nv_shuffle_index(rand_idx, 0, pc);
#if 1
{
float max_x, max_y;
if (pc < sctx->sctx->list) {
// 足りないときはランダムに増やす
for (i = pc; i < sctx->sctx->list; ++i) {
rand_idx[i] = (int)(nv_rand() * pn);
}
}
pc = pn = sctx->sctx->list;
// 半径を求める
max_x = 0.0f;
max_y = 0.0f;
for (m = 0; m < pn; ++m) {
float yd = fabsf(NV_MAT_V(points, rand_idx[m], 0) - u_y);
float xd = fabsf(NV_MAT_V(points, rand_idx[m], 1) - u_x);
max_x = NV_MAX(max_x, xd);
max_y = NV_MAX(max_y, yd);
}
r = (float)img->rows/2.0f;//NV_MAX(max_x, max_y) * 1.0f;
}
#endif
// log(r) = 5の基底定数を求める
r_e = powf(r, 1.0f / NV_SC_LOG_R_BIN);
// histgramを計算する
sctx->n = pn;
nv_matrix_zero(sctx->sctx);
nv_matrix_zero(sctx->tan_angle);
for (l = 0; l < pn; ++l) {
// tangent angle
#if 0
float max_bin = 0.0f, min_bin = FLT_MAX;
float tan_angle = tangent_angle(
r,
NV_MAT_V(points, rand_idx[l], 0),
NV_MAT_V(points, rand_idx[l], 1),
points, pc);
#else
float tan_angle = 0.0f;
#endif
p_y = NV_MAT_V(points, rand_idx[l], 0);
p_x = NV_MAT_V(points, rand_idx[l], 1);
NV_MAT_V(sctx->tan_angle, l, 0) = tan_angle;
NV_MAT_V(sctx->coodinate, l, 0) = p_y;
NV_MAT_V(sctx->coodinate, l, 1) = p_x;
NV_MAT_V(sctx->radius, l, 0) = r;
// shape context
for (i = 0; i < pn; ++i) {
// # i ≠ l判定はとりあえずしない
float xd = NV_MAT_V(points, rand_idx[i], 1) - p_x;
float yd = NV_MAT_V(points, rand_idx[i], 0) - p_y;
//int row = i / img->rows;
//int col = i % img->rows;
//float xd = col - p_x;
//float yd = row - p_y;
float theta;
float log_r = logf(sqrtf(xd * xd + yd * yd)) / logf(r_e);
float atan_r = atan2f(xd, yd);
//if (NV_MAT3D_V(img, row, col, 0) == 0.0f) {
// continue;
//}
if (i == l) {
continue;
}
if (atan_r < 0.0f) {
atan_r = 2.0f * NV_PI + atan_r;
}
if (tan_angle > 0.0f) {
if (atan_r + tan_angle > 2.0f * NV_PI) {
atan_r = atan_r + tan_angle - 2.0f * NV_PI;
} else {
atan_r += tan_angle;
}
} else {
if (atan_r + tan_angle < 0.0f) {
atan_r = 2.0f * NV_PI + (atan_r + tan_angle);
} else {
atan_r += tan_angle;
}
}
theta = atan_r / (2.0f * NV_PI / NV_SC_THETA_BIN);
if (theta < NV_SC_THETA_BIN && log_r < NV_SC_LOG_R_BIN) {
NV_MAT3D_LIST_V(sctx->sctx, l, (int)log_r, (int)theta, 0) += 1.0f;
}
}
#if 0
for (row = 0; row < NV_SC_LOG_R_BIN; ++row) {
for (col = 0; col < NV_SC_THETA_BIN; ++col) {
max_bin = NV_MAX(max_bin, NV_MAT3D_LIST_V(sctx->sctx, l, row, col, 0));
min_bin = NV_MIN(min_bin, NV_MAT3D_LIST_V(sctx->sctx, l, row, col, 0));
}
}
if (max_bin > 0.0f) {
for (row = 0; row < NV_SC_LOG_R_BIN; ++row) {
for (col = 0; col < NV_SC_THETA_BIN; ++col) {
NV_MAT3D_LIST_V(sctx->sctx, l, row, col, 0)
= (NV_MAT3D_LIST_V(sctx->sctx, l, row, col, 0) - min_bin) / (max_bin - min_bin);
}
}
}
#endif
}
nv_matrix_free(&edge);
nv_matrix_free(&points);
nv_free(rand_idx);
}
