owlps/scripts/OwlPS/TimeInterpolation.pm

# This file is part of the Owl Positioning System (OwlPS) project.  It is
# subject to the copyright notice and license terms in the COPYRIGHT.t2t file
# found in the top-level directory of this distribution and at
# https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
# No part of the OwlPS Project, including this file, may be copied, modified,
# propagated, or distributed except according to the terms contained in the
# COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be distributed along with this
# file, either separately or by replacing this notice by the COPYRIGHT.t2t
# file's contents.


=head1 NAME

OwlPS::TimeInterpolation - interpolate timestamped coordinates


=head1 SYNOPSIS

    use OwlPS::TimeInterpolation;

    interpolate_1d(1,        "1368637546.331881794",
                   10,       "1368637879.566242145",
                             "1368637726.677787231");
    interpolate_2d("1;2",    "1368637546.331881794",
                   "10;5",   "1368637879.566242145",
                             "1368637726.677787231");
    interpolate_3d("1;2;1",  "1368637546.331881794",
                   "10;5;2", "1368637879.566242145",
                             "1368637726.677787231");


=head1 DESCRIPTION

The OwlPS::TimeInterpolation Perl module offers function to interpolate
timestamped coordinates. Its main functions are interpolate_1d(),
interpolate_2d() and interpolate_3d() (cf. section L</"Main functions"> below).

=head2 Helper functions (not exported by default)

The following functions are provided but not exported by default.

=cut


package OwlPS::TimeInterpolation;
require Exporter;

our @ISA    = qw(Exporter);
our @EXPORT = qw(
  interpolate_1d
  interpolate_2d
  interpolate_3d
);
our @EXPORT_OK = qw(
  timestamp_cmp
  elapsed
  elapsed_sec
  travel_speed
  travel_speed_1d
  distance_travelled
  linear_interpolation
);
our $VERSION = 1.00;

use strict;
use Carp;
use OwlPS::CSV;
use Scalar::Util qw(looks_like_number);


=head3 Time-related functions

=over

=cut


=item timestamp_cmp($t0, $t1)

This function compares two timestamps $t0 and $t1, which are strings under the
format "seconds.nanoseconds".
It returns:

=over

=item * 1 if $t0 is before $t1

=item * -1 if $t0 is after $t1

=item * 0 if $t0 and $t1 are equal

=back
=cut
sub timestamp_cmp($$) {
    my ($t0,     $t1)      = @_;
    my ($t0_sec, $t0_nsec) = split_timestamp($t0);
    my ($t1_sec, $t1_nsec) = split_timestamp($t1);

    # First test the seconds
    if ($t1_sec > $t0_sec) { return 1 }
    if ($t1_sec < $t0_sec) { return -1 }

    # Seconds are equal, test the nanoseconds
    if ($t1_nsec > $t0_nsec) { return 1 }
    if ($t1_nsec < $t0_nsec) { return -1 }

    # Seconds and nanoseconds are equal
    return 0;
}


=item elapsed($t0, $t1)

This function calculates the elapsed time between two timestamps $t0 and $t1
(string format: "seconds.nanoseconds").
It returns an array ($seconds, $nanoseconds).
=cut
sub elapsed($$) {
    my ($t0,     $t1)      = @_;
    my ($t0_sec, $t0_nsec) = split_timestamp($t0);
    my ($t1_sec, $t1_nsec) = split_timestamp($t1);

    my $sec  = $t1_sec - $t0_sec;
    my $nsec = $t1_nsec - $t0_nsec;

    if ($sec == 0) { return (0, abs($nsec)) }

    if ($sec > 0) {
        if ($nsec >= 0) { return ($sec, $nsec) }

        # $nsec < 0
        return ($sec - 1, $nsec + 1000000000);
    }

    # $sec < 0
    if ($nsec > 0) { return (abs($sec) - 1, 1000000000 - $nsec) }

    # $nsec <= 0
    return (abs($sec), abs($nsec));
}


=item elapsed_sec($t0, $t1)

This function calculates the elapsed time between two timestamps $t0 and $t1, in
seconds.
It returns the number of seconds (floating point value).
=cut
sub elapsed_sec($$) {
    my ($t0, $t1) = @_;
    my ($t0t1_sec, $t0t1_nsec) = elapsed($t0, $t1);
    my $total_nsec = $t0t1_sec * 1000000000 + $t0t1_nsec;
    return $total_nsec / 1000000000;
}


=back

=head3 Distance and speed-related functions

=over

=cut


=item travel_speed($start_time, $stop_time, $distance)

This function returns the movement speed, given two timestamp $start_time and
$stop_time and a travelled distance $distance.
=cut
sub travel_speed($$$) {
    my ($start_time, $stop_time, $distance) = @_;
    my $time_elapsed = elapsed_sec($start_time, $stop_time);
    return $distance / $time_elapsed;
}


=item travel_speed_1d($start_point, $start_time, $stop_point, $stop_time)

This function returns the movement speed between two coordinates (1-D points),
given the start position $start_point at a timestamp $start_time, and the stop
position $stop_point at a timestamp $stop_time.
=cut
sub travel_speed_1d($$$$) {
    my ($start_point, $start_time, $stop_point, $stop_time) = @_;
    if (!looks_like_number($start_point) || !looks_like_number($stop_point)) {
        croak "1-D coordinates must be numbers";
    }
    my $distance_travelled = abs($stop_point - $start_point);
    return travel_speed($start_time, $stop_time, $distance_travelled);
}


=item distance_travelled($start_time, $stop_time, $speed)

This function returns the distance travelled between two timestamps $start_time
and $stop_time, given the movement speed $speed.
=cut
sub distance_travelled($$$) {
    my ($start_time, $stop_time, $speed) = @_;
    my $time_travelled = elapsed_sec($start_time, $stop_time);
    return $time_travelled * $speed;
}


=back

=head3 Interpolation-related functions

=over

=cut


=item linear_interpolation($start_point, $stop_point, $distance)

Given two 1-D coordinates $start_point and $stop_point, this function computes
the interpolated coordinate at $distance from $start_point in the direction of
$stop_point.
It returns the interpolated 1-D coordinate.
=cut
sub linear_interpolation($$$) {
    my ($start_point, $stop_point, $distance) = @_;
    if (!looks_like_number($start_point) || !looks_like_number($stop_point)) {
        croak "1-D coordinates must be numbers";
    }
    if ($start_point <= $stop_point) {
        return $start_point + $distance;
    }
    return $start_point - $distance;
}


=back

=head2 Main functions

The following functions are exported by default.

=over

=cut


=item interpolate_1d($start_point, $start_time, $stop_point, $stop_time, $time)

Given two 1-D coordinates $start_point and $stop_point with their associated
timestamps $start_time and $stop_time, and an additional timestamp $time, this
function computes the coordinates associated with this additional timestamp. A
constant pace is assumed between the two timestamped coordinates. $time should
be after the $start_time, and can also be after $stop_time.
The interpolated coordinate is returned.
=cut
sub interpolate_1d($$$$$) {
    my ($start_point, $start_time, $stop_point, $stop_time, $time) = @_;

    # Check the timestamps
    if (timestamp_cmp($start_time, $stop_time) <= 0) {
        croak "\$start_time must be before \$stop_time";
    }
    if (timestamp_cmp($start_time, $time) < 0) {
        croak "\$start_time must be equal or before \$time";
    }

    # Compute the travel speed between the two known points
    my $speed =
      travel_speed_1d($start_point, $start_time, $stop_point, $stop_time);

    # Compute the distance travelled between `start_point` and the point
    # to interpolate
    my $distance = distance_travelled($start_time, $time, $speed);

    # Compute and return the interpolated coordinates
    return linear_interpolation($start_point, $stop_point, $distance);
}


=item interpolate_2d($start_point, $start_time, $stop_point, $stop_time, $time)

This function is similar to interpolate_1d(), but works with 2-D coordinates:
$start_point and $stop_point must be strings "X;Y".
It returns an array ($x, $y) representing the interpolated point.
=cut
sub interpolate_2d($$$$$) {
    my ($start_point, $start_time, $stop_point, $stop_time, $time) = @_;

    my ($start_x, $start_y) = split_point_2d($start_point);
    my ($stop_x,  $stop_y)  = split_point_2d($stop_point);

    return (
        interpolate_1d($start_x, $start_time, $stop_x, $stop_time, $time),
        interpolate_1d($start_y, $start_time, $stop_y, $stop_time, $time)
    );
}


=item interpolate_3d($start_point, $start_time, $stop_point, $stop_time, $time)

This function is similar to interpolate_1d() and interpolate_2d(), but works
with 3-D coordinates: $start_point and $stop_point must be strings "X;Y;Z".
It returns an array ($x, $y, $z) representing the interpolated point.
=cut
sub interpolate_3d($$$$$) {
    my ($start_point, $start_time, $stop_point, $stop_time, $time) = @_;

    my ($start_x, $start_y, $start_z) = split_point_3d($start_point);
    my ($stop_x,  $stop_y,  $stop_z)  = split_point_3d($stop_point);

    return (
        interpolate_1d($start_x, $start_time, $stop_x, $stop_time, $time),
        interpolate_1d($start_y, $start_time, $stop_y, $stop_time, $time),
        interpolate_1d($start_z, $start_time, $stop_z, $stop_time, $time)
    );
}


=back

=head1 COPYING

This module and its documentation are part of the Owl Positioning System (OwlPS)
project. They are subject to the copyright notice and license terms in the
COPYRIGHT.t2t file found in the top-level directory of the OwlPS distribution
and at https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t


=head1 SEE ALSO

owlps(7)

=cut


# vim: tabstop=4:shiftwidth=4:expandtab:textwidth=80