MotionProfile.cpp

#include "MotionProfile.h"
#include <math.h>
#include <assert.h>
#include "DebugUtil.h"

MotionProfile::MotionProfile()
{
	times.clear();
	values.clear();
	time_scale = 1;
	value_scale = 1;
}

void MotionProfile::AddPoint(double time, double value)
{
	if(!times.empty()){
		double last_time = times[times.size() - 1];
		if(time > last_time){
			DPRINTF("Added point: (%2.2f,%2.2f)\n", time, value);
			times.push_back(time);
			values.push_back(value);
		}
	}else
	{
		DPRINTF("Added point: (%2.2f,%2.2f)\n", time, value);
		times.push_back(time);
		values.push_back(value);
	}

}

void MotionProfile::SetTimeScale(double scale)
{
	time_scale = scale;
}

void MotionProfile::SetValueScale(double scale)
{
	value_scale = scale;
}

double MotionProfile::GetInterpolatedValue(double time)
{
	int first_idx = -1;
	int second_idx = -1;
	int current = 0;
	double interpolate = 0;
	double normalized_time = time / time_scale;
	
	for(int i = 0; i < (int)times.size(); i++){
		if(times[i] <= normalized_time) first_idx = i;
		if(times[times.size()-1-i] >= normalized_time) second_idx = times.size()-1-i;
	}
	
	if (first_idx == -1) { // We didn't find a "first" time
		DPRINTF("First special case encountered, we are before the first point.\n", NULL);
		first_idx = 0; // Assume we are before the first point, set it to 0
	}
	if (second_idx == -1) {
		DPRINTF("Second special case encountered, we are after the last point.\n", NULL);
		second_idx = times.size() - 1;
	}

	if(first_idx == second_idx) interpolate = values[first_idx];
	else{
		double time_difference = times[second_idx] - times[first_idx];
		double mu = (normalized_time - times[first_idx]) / time_difference;
		interpolate = (values[first_idx]*(1-mu)+values[second_idx]*mu);
	}
	DPRINTF("first = %d\tsecond = %d\n", first_idx, second_idx);
	assert(abs(first_idx - second_idx) <= 1);
	
	return interpolate*value_scale;
}

double MotionProfile::GetCosineInterpolatedValue(double time)
{
	int first_idx = -1;
	int second_idx = -1;
	int current = 0;
	double interpolate = 0;
	double normalized_time = time / time_scale;
	
	for(int i = 0; i < (int)times.size(); i++){
		if(times[i] <= normalized_time) first_idx = i;
		if(times[times.size()-1-i] >= normalized_time) second_idx = times.size()-1-i;
	}
	

	if (first_idx == -1) { // We didn't find a "first" time
		DPRINTF("First special case encountered, we are before the first point.\n", NULL);
		first_idx = 0; // Assume we are before the first point, set it to 0
	}
	if (second_idx == -1) {
		DPRINTF("Second special case encountered, we are after the last point.\n", NULL);
		second_idx = times.size() - 1;
	}

	if(first_idx == second_idx) interpolate = values[first_idx];
	else{
		double time_difference = times[second_idx] - times[first_idx];
		double mu = (normalized_time - times[first_idx]) / time_difference;
		double mu2 = (1.0-cos(mu*3.14159))/2.0;
		interpolate = (values[first_idx]*(1-mu2)+values[second_idx]*mu2);
	}
	DPRINTF("first = %d\tsecond = %d\n", first_idx, second_idx);
		assert(abs(first_idx - second_idx) <= 1);
	return interpolate*value_scale;	
}