cuttraj.cc

#include "cuttraj.h"
#include <iostream>
#include <armadillo>

using namespace arma;



void cuttraj(const mat& evalTraj, const mat& corrTraj, mat& traj1, mat& traj2) {
	// Distances (2-norm):
	mat dists = zeros<mat>(evalTraj.n_rows, corrTraj.n_rows);
	for (int oldp = 0; oldp < evalTraj.n_rows; ++oldp) {
		for (int newp = 0; newp < corrTraj.n_rows; ++newp) {
			dists(oldp, newp) = norm( evalTraj.row(oldp) - corrTraj.row(newp));
		}
	}
	mat dist2old = min(dists);
	mat dist2oldmax = max(dist2old,1);
	
	double th = 0.5;
	int cutnew = 0;
	bool haslonger = false;
	for (int newp = dist2old.n_cols; newp > 0; --newp) {
		if (dist2old(newp-1) > th*dist2oldmax(0,0)) {
			cutnew = newp;
			haslonger = 1;
		} else if (haslonger) {
			break;
		}
	}
	
	// Cut old trajectory:
	mat dist2cutnew = dists.col(cutnew);
	
	double mindist2cutnew = as_scalar(min(dist2cutnew));
	cout << mindist2cutnew << endl;

	double cutold = as_scalar(find(dist2cutnew == mindist2cutnew, 1, "first")) + 1;
	
	
	traj1 = evalTraj.rows(0,cutold-1);
	traj2 = corrTraj.rows(cutnew-1, corrTraj.n_rows -1);
	

}