bool nav_mesh::find(const float* start, const float* end, std::vector<path_step>& path)
{
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
path.clear();
dtStatus status = query_->findNearestPoly(start, polyPickExt, &filter_, &startRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
status = query_->findNearestPoly(end, polyPickExt, &filter_, &endRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
dtPolyRef polys[MAX_POLYS] = { 0, };
int npolys = 0;
status = query_->findPath(startRef, endRef, start, end, &filter_, polys, &npolys, sizeof(polys) / sizeof(polys[0]));
if (dtStatusFailed(status))
{
return false;
}
if (npolys == 0 || polys[npolys - 1] != endRef)
{
return false;
}
float straightPos[MAX_POLYS * 3] = { 0, };
unsigned char straightFlags[MAX_POLYS] = { 0, };
dtPolyRef straightPolys[MAX_POLYS] = { 0, };
int nstraightPath = 0;
status = query_->findStraightPath(start, end, polys, npolys,
straightPos, straightFlags,
straightPolys, &nstraightPath, MAX_POLYS, 0);
if (dtStatusFailed(status))
{
return false;
}
for (int i = 0; i < nstraightPath; ++i)
{
const float* step = straightPos + (3 * i);
path.emplace_back(path_step(step));
}
return true;
}
/* COMPARE_CHROMA
Perform Dynamic Programming to find optimal alignment
*/
int Scorealign::compare_chroma()
{
float *path;
/* Allocate the distance matrix */
path = (float *) calloc(file0_frames * file1_frames, sizeof(float));
/* skip over initial silence in signals */
if (ignore_silence) {
first_x = frames_of_init_silence(chrom_energy0, file0_frames);
last_x = last_non_silent_frame(chrom_energy0, file0_frames);
first_y = frames_of_init_silence(chrom_energy1, file1_frames);
last_y = last_non_silent_frame(chrom_energy1, file1_frames);
} else {
first_x = 0;
last_x = file0_frames - 1;
first_y = 0;
last_y = file1_frames - 1;
}
if (last_x - first_x <= 0 || last_y - first_y <= 0) {
return SA_TOOSHORT;
}
/* Initialize first row and column */
if (verbose) printf("Performing DP\n");
PATH(first_x, first_y) = gen_dist(first_x, first_y);
for (int x = first_x + 1; x <= last_x; x++)
PATH(x, first_y) = gen_dist(x, first_y) + PATH(x - 1, first_y);
for (int y = 1; y <= last_y; y++)
PATH(first_x, y) = gen_dist(first_x, y) + PATH(first_x, y - 1);
#if DEBUG_LOG
fprintf(dbf, "DISTANCE MATRIX ***************************\n");
#endif
/* Perform DP for the rest of the matrix */
for (int x = first_x + 1; x <= last_x; x++) {
for (int y = first_y + 1; y <= last_y; y++) {
PATH(x, y) = gen_dist(x, y) +
float(min3(PATH(x-1, y-1), PATH(x-1, y), PATH(x, y-1)));
#if DEBUG_LOG
fprintf(dbf, "(%d %d %g) ", x, y, gen_dist(x, y), PATH(x, y));
#endif
}
#if DEBUG_LOG
fprintf(dbf, "\n");
#endif
// report progress for each file0_frame (column)
// This is not quite right if we are ignoring silence because
// then only a sub-matrix is computed.
if (progress && !progress->set_matrix_progress(file1_frames))
return SA_CANCEL;
}
#if DEBUG_LOG
fprintf(dbf, "END OF DISTANCE MATRIX ********************\n");
#endif
if (verbose) printf("Completed Dynamic Programming.\n");
//x and y are the ending points, it can end at either the end of midi,
// or end of audio or both
pathx = ALLOC(short, (file0_frames + file1_frames));
pathy = ALLOC(short, (file0_frames + file1_frames));
assert(pathx != NULL);
assert(pathy != NULL);
// map from file0 time to file1 time
time_map = ALLOC(float, file0_frames);
smooth_time_map = ALLOC(float, file0_frames);
int x = last_x;
int y = last_y;
if (!force_final_alignment) {
#if DEBUG_LOG
fprintf(dbf, "\nOptimal Path: ");
#endif
// find end point, the lowest cost matrix value at one of the
// sequence endings
float min_cost = 1.0E10;
for (int i = first_x; i <= last_x; i++) {
if (PATH(i, last_y) <= min_cost) {
min_cost = PATH(i, last_y);
x = i;
y = last_y;
}
}
for (int j = first_y; j <= last_y; j++) {
if (PATH(last_x, j) <= min_cost) {
min_cost = PATH(last_x, j);
x = last_x;
y = j;
}
}
#if DEBUG_LOG
fprintf(dbf, "Min cost at %d %d\n\nPATH:\n", x, y);
#endif
}
while ((x != first_x) || (y != first_y)) {
path_step(x, y);
/* Check for the optimal path backwards*/
if (x > first_x && y > first_y && PATH(x-1, y-1) <= PATH(x-1, y) &&
PATH(x-1, y-1) <= PATH(x, y-1)) {
x--;
y--;
} else if (x > first_x && y > first_y && PATH(x-1, y) <= PATH(x, y-1)) {
x--;
} else if (y > first_y) {
y--;
} else if (x > first_x) {
x--;
}
}
path_step(x, y);
path_reverse();
free(path);
return SA_SUCCESS; // success
}
