#!/usr/bin/perl -w
# vi:set nowrap ts=4:
#
# $Id: Roller.pm,v 0.1 2002/09/10 06:28:47 roc Exp $
#
#
package Roller;

use strict;
use Math::Trig;

$Roller::Fs = 100;	# samples per second
$Roller::h  = 2 * $Roller::Fs; # bilinear transformation constant \
		 	  		           # (i.e., Adams-Moulton 1 step integration)

$Roller::tmax = 0.5;     # maximum value of the parameter (for parametric curve), in meters
                         # parameter goes from -tmax to tmax
$Roller::inct = $Roller::tmax * 2 / 128;    # linear increment to express the curve in 
                         # parametric form
$Roller::tdipmax = 0.05; # half width of the bottom tip
$Roller::gravity = 9.8;  # vertical acceleration in m/s^2
$Roller::damping = 0.4;  # damping over mass in in 1/s


sub build_index($$)
{
	my ($max, $inc) = @_;
	my @result = ();

	for (my $idx = 0, my $val = -$max; $val < $max; ++$idx, $val += $inc)
	{
		$result[$idx] = $val;
	}

	return \@result;
}
#
# Parametric curve: (x(t), y(t))
#
sub build_parametric($$)
{
	my ($r_x,$dipmax) = @_;
	my @y = ();

	for (my $i = 0; $i < scalar(@{$r_x}); ++$i)
	{
		$y[$i] = log(abs($r_x->[$i]*(abs($r_x->[$i])>=$dipmax))+1);
		$y[$i] = $y[$i] + ($y[$i] == 0) * log($dipmax+1);
		$y[$i] **= 2;
	}

	return \@y;
}
sub build_pinc($$)
{
	my ($r_x, $r_y) = @_;
	my @pinc = ();
	my $end = scalar(@{$r_x});
	
	#
	# Here I suppose that the size of @x and @y are always the same
	#
	for (my $i = 1; $i < $end; ++$i)
	{
		$pinc[$i-1] = sqrt(($r_x->[$i]-$r_x->[$i-1])**2 + ($r_y->[$i]-$r_y->[$i-1])**2);
	}

	return \@pinc;
}
sub build_pslope($$)
{
	my ($r_x, $r_y) = @_;
	my @pslope = ();
	my $end = scalar(@{$r_x});

	for (my $i = 1; $i < $end; ++$i)
	{
		$pslope[$i] = ($r_y->[$i] - $r_y->[$i-1]) / ($r_x->[$i] - $r_x->[$i-1]);
	}

	$pslope[0] = $pslope[1];

	return \@pslope;
}
sub build_p($)
{
	my ($r_pinc) = @_;
	my @p = ();
	my $end = scalar(@{$r_pinc});

	$p[0] = 0;
	for (my $i = 0; $i < $end; ++$i)
	{
		$p[$i+1] = $p[$i] + $r_pinc->[$i];
	}

	return \@p;
}
sub build_sigma($$)
{
	my ($r_pslope, $gravity) = @_;
	my @sigma = ();
	my $end = scalar(@{$r_pslope});

	for (my $i = 0; $i < $end; ++$i)
	{
		$sigma[$i] = -$gravity * sin(atan($r_pslope->[$i]));
	}

	return \@sigma;
}
sub build_ptilde($$$$)
{
	my ($r_p, $r_sigma, $h, $damping) = @_;
	my @ptilde = ();
	my $end = scalar(@{$r_p});

	for (my $i = 0; $i < $end; ++$i)
	{
		$ptilde[$i] = $r_p->[$i] - 1/($h * ($h + $damping)) * $r_sigma->[$i];
	}

	return \@ptilde;
}

1;
