335 lines
9.1 KiB
Perl
335 lines
9.1 KiB
Perl
|
# 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
|
||
|
# http://code.lm7.fr/p/owlps/source/tree/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 http://code.lm7.fr/p/owlps/source/tree/master/COPYRIGHT.t2t
|
||
|
|
||
|
|
||
|
=head1 SEE ALSO
|
||
|
|
||
|
owlps(7)
|
||
|
|
||
|
=cut
|
||
|
|
||
|
|
||
|
# vim: tabstop=4:shiftwidth=4:expandtab:textwidth=80
|