File Coverage

lib/Geo/Spline.pm

Criterion	Covered	Total	%
statement	89	89	100.0
branch	11	12	91.6
condition	3	6	50.0
subroutine	12	12	100.0
pod	5	7	71.4
total	120	126	95.2

package Geo::Spline;

=head1 NAME

Geo::Spline - Calculate geographic locations between GPS fixes.

=head1 SYNOPSIS

  use Geo::Spline;
  my $p0={time=>1160449100.67, #seconds
          lat=>39.197807,      #degrees
          lon=>-77.263510,     #degrees
          speed=>31.124,       #m/s
          heading=>144.8300};  #degrees clockwise from North
  my $p1={time=>1160449225.66,
          lat=>39.167718,
          lon=>-77.242278,
          speed=>30.615,
          heading=>150.5300};
  my $spline=Geo::Spline->new($p0, $p1);
  my %point=$spline->point(1160449150);
  print "Lat:", $point{"lat"}, ", Lon:", $point{"lon"}, "\n\n";

  my @points=$spline->pointlist();
  foreach (@points) {
    print "Lat:", $_->{"lat"}, ", Lon:", $_->{"lon"}, "\n";
  }

=head1 DESCRIPTION

This program was developed to be able to calculate the position between two GPS fixes using a 2-dimensional 3rd order polynomial spline.

  f(t)  = A + B(t-t0)  + C(t-t0)^2 + D(t-t0)^3 #position in X and Y
  f'(t) = B + 2C(t-t0) + 3D(t-t0)^2            #velocity in X and Y

I did some simple Math (for an engineer with a math minor) to come up with these formulas to calculate the unknowns from our knowns.

  A = x0                                     # when (t-t0)=0 in f(t)
  B = v0                                     # when (t-t0)=0 in f'(t)
  C = (x1-A-B(t1-t0)-D(t1-t0)^3)/(t1-t0)^2   # solve for C from f(t)
  C = (v1-B-3D(t1-t0)^2)/2(t1-t0)            # solve for C from f'(t)
  D = (v1(t1-t0)+B(t1-t0)-2x1+2A)/(t1-t0)^3  # equate C=C then solve for D

=cut

use strict;
use vars qw($VERSION);
use Geo::Constants qw{PI};
use Geo::Functions qw{deg_rad rad_deg round};

$VERSION = sprintf("%d.%02d", q{Revision: 0.16} =~ /(\d+)\.(\d+)/);

=head1 CONSTRUCTOR

=head2 new

  my $spline=Geo::Spline->new($p0, $p1);

=cut

sub new {
  my $this = shift();
  my $class = ref($this) || $this;
  my $self = {};
  bless $self, $class;
  $self->initialize(@_);
  return $self;
}

=head1 METHODS

=cut

sub initialize {
  my $self = shift();
  $self->{'pt0'}=shift();
  $self->{'pt1'}=shift();
  my $ellipsoid=$self->ellipsoid("WGS84");
  my $dt=$self->{'pt1'}->{'time'} - $self->{'pt0'}->{'time'};
  die ("Delta time must be greater than zero.") if ($dt<=0);
  my ($A, $B, $C, $D)=$self->ABCD(
     $self->{'pt0'}->{'time'},
     $self->{'pt0'}->{'lat'} * $ellipsoid->polar_circumference / 360,
     $self->{'pt0'}->{'speed'} * cos(rad_deg($self->{'pt0'}->{'heading'})),
     $self->{'pt1'}->{'time'},
     $self->{'pt1'}->{'lat'} * $ellipsoid->polar_circumference / 360,
     $self->{'pt1'}->{'speed'} * cos(rad_deg($self->{'pt1'}->{'heading'})));
  $self->{'Alat'}=$A;
  $self->{'Blat'}=$B;
  $self->{'Clat'}=$C;
  $self->{'Dlat'}=$D;
  ($A, $B, $C, $D)=$self->ABCD(
     $self->{'pt0'}->{'time'},
     $self->{'pt0'}->{'lon'} * $ellipsoid->equatorial_circumference / 360,
     $self->{'pt0'}->{'speed'} * sin(rad_deg($self->{'pt0'}->{'heading'})),
     $self->{'pt1'}->{'time'},
     $self->{'pt1'}->{'lon'} * $ellipsoid->equatorial_circumference / 360,
     $self->{'pt1'}->{'speed'} * sin(rad_deg($self->{'pt1'}->{'heading'})));
  $self->{'Alon'}=$A;
  $self->{'Blon'}=$B;
  $self->{'Clon'}=$C;
  $self->{'Dlon'}=$D;
}

=head2 ellipsoid

Method to set or retrieve the current ellipsoid object.  The ellipsoid is a Geo::Ellipsoids object.

  my $ellipsoid=$obj->ellipsoid;  #Default is WGS84

  $obj->ellipsoid('Clarke 1866'); #Built in ellipsoids from Geo::Ellipsoids
  $obj->ellipsoid({a=>1});        #Custom Sphere 1 unit radius

=cut

sub ellipsoid {
  my $self = shift();
  if (@_) {
    my $param=shift();
    use Geo::Ellipsoids;
    my $obj=Geo::Ellipsoids->new($param);
    $self->{'ellipsoid'}=$obj;
  }
  return $self->{'ellipsoid'};
}

sub ABCD {
  my $self = shift();
  my $t0 = shift();
  my $x0 = shift();
  my $v0 = shift();
  my $t1 = shift();
  my $x1 = shift();
  my $v1 = shift();
  #x=f(t)=A+B(t-t0)+C(t-t0)^2+D(t-t0)^3
  #v=f'(t)=B+2C(t-t0)+3D(t-t0)^2
  #A=x0
  #B=v0
  #C=(x1-A-B(t1-t0)-D(t1-t0)^3)/((t1-t0)^2) # from f(t)
  #C=(v1-B-3D(t1-t0)^2)/2(t1-t0)            # from f'(t)
  #D=(v1t+Bt-2x1+2A)/t^3                    # from C=C
  my $A=$x0;
  my $B=$v0;
  #=(C3*(A3-A2)+B6*(A3-A2)-2*B3+2*B5)/(A3-A2)^3 # for Excel
  my $D=($v1*($t1-$t0)+$B*($t1-$t0)-2*$x1+2*$A)/($t1-$t0)**3;
  #=(B3-B5-B6*(A3-A2)-B8*(A3-A2)^3)/(A3-A2)^2   # for Excel
  my $C=($x1-$A-$B*($t1-$t0)-$D*($t1-$t0)**3)/($t1-$t0)**2;
  return($A,$B,$C,$D);
}

=head2 point

Method returns a single point from a single time.

  my $point=$spline->point($t1);
  my %point=$spline->point($t1);

=cut

sub point {
  my $self=shift();
  my $timereal=shift();
  my $ellipsoid=$self->ellipsoid;
  my $t=$timereal-$self->{'pt0'}->{'time'};
  my ($Alat, $Blat, $Clat, $Dlat)=($self->{'Alat'}, $self->{'Blat'},$self->{'Clat'},$self->{'Dlat'});
  my ($Alon, $Blon, $Clon, $Dlon)=($self->{'Alon'}, $self->{'Blon'},$self->{'Clon'},$self->{'Dlon'});
  my $lat=$Alat + $Blat * $t + $Clat * $t ** 2 + $Dlat * $t ** 3;
  my $lon=$Alon + $Blon * $t + $Clon * $t ** 2 + $Dlon * $t ** 3;
  my $vlat=$Blat + 2 * $Clat * $t + 3 * $Dlat * $t ** 2;
  my $vlon=$Blon + 2 * $Clon * $t + 3 * $Dlon * $t ** 2;
  my $speed=sqrt($vlat ** 2 + $vlon ** 2);
  my $heading=PI()/2 - atan2($vlat,$vlon);
  $heading=deg_rad($heading);
  $lat/=$ellipsoid->polar_circumference / 360;
  $lon/=$ellipsoid->equatorial_circumference / 360;
  my %pt=(time=>$timereal,
          lat=>$lat,
          lon=>$lon,
          speed=>$speed,
          heading=>$heading);
  return wantarray ? %pt : \%pt;
}

=head2 pointlist

Method returns a list of points from a list of times.

  my $list=$spline->pointlist($t1,$t2,$t3);
  my @list=$spline->pointlist($t1,$t2,$t3);

=cut

sub pointlist {
  my $self=shift();
  my @list=@_;
  @list=$self->timelist() if (scalar(@list)== 0);
  my @points=();
  foreach (@list) {
    push @points, {$self->point($_)};
  }
  return wantarray ? @points : \@points;
}

=head2 timelist

Method returns a list of times (n+1).  The default will return a list with an integer number of seconds between spline end points.

  my $list=$spline->timelist($samples);
  my @list=$spline->timelist();

=cut

sub timelist {
  my $self=shift();
  my $t0=$self->{'pt0'}->{'time'};
  my $t1=$self->{'pt1'}->{'time'};
  my $dt=$t1-$t0;
  my $count=shift() || round($dt);
  my @list;
  foreach(0..$count) {
    my $t=$t0+$dt*($_/$count);
    push @list, $t;
  }
  return wantarray ? @list : \@list;
}

1;

__END__