diff --git a/etc/auv/simulator.ini b/etc/auv/simulator.ini index f8537d17e7..9e9450de50 100644 --- a/etc/auv/simulator.ini +++ b/etc/auv/simulator.ini @@ -143,16 +143,14 @@ Maximum Deviation - Horizontal = 1.0 Maximum Deviation - Bearing = 20 PRNG Seed = 100 -[Simulators.Gaussian/Temperature] +[Simulators.CTD] Enabled = Simulation Entity Label = CTD Simulator -Name of message to produce = Temperature -Invalid at Surface = false - -[Simulators.Gaussian/SoundSpeed] -Enabled = Simulation -Entity Label = Sound Speed Simulator -Name of message to produce = SoundSpeed -Value at peak = 1522 -Value away from peak = 1500 -Invalid at Surface = true +Execution Frequency = 6 +Model Type = Constant +# Model Type = MOHID Model Data +# Data File Path = +Default - Temperature = 5.0 +Default - Salinity = 10.0 +Interpolation Radius - Surface = 0.01 +Interpolation Radius - Depth = 15 \ No newline at end of file diff --git a/programs/scripts/dune-create-data-file.py b/programs/scripts/dune-create-data-file.py new file mode 100644 index 0000000000..ce05c35d54 --- /dev/null +++ b/programs/scripts/dune-create-data-file.py @@ -0,0 +1,284 @@ +# -*- coding: utf-8 -*- +############################################################################ +# Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia # +# Laboratório de Sistemas e Tecnologia Subaquática (LSTS) # +############################################################################ +# This file is part of DUNE: Unified Navigation Environment. # +# # +# Commercial Licence Usage # +# Licencees holding valid commercial DUNE licences may use this file in # +# accordance with the commercial licence agreement provided with the # +# Software or, alternatively, in accordance with the terms contained in a # +# written agreement between you and Faculdade de Engenharia da # +# Universidade do Porto. For licensing terms, conditions, and further # +# information contact lsts@fe.up.pt. # +# # +# Modified European Union Public Licence - EUPL v.1.1 Usage # +# Alternatively, this file may be used under the terms of the Modified # +# EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md # +# included in the packaging of this file. You may not use this work # +# except in compliance with the Licence. Unless required by applicable # +# law or agreed to in writing, software distributed under the Licence is # +# distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF # +# ANY KIND, either express or implied. See the Licence for the specific # +# language governing permissions and limitations at # +# https://github.com/LSTS/dune/blob/master/LICENCE.md and # +# http://ec.europa.eu/idabc/eupl.html. # +############################################################################ +# Author: Luis Venancio # +############################################################################ + +import argparse +import time +import math +import h5py + +import numpy as np +from scipy.interpolate import griddata + +################################################# Auxiliar Functions ################################################## +# Coordinate transformation +def WGS84toNED(rlat, rlon, rdep, lat, lon, dep): + + rlat = math.radians(rlat) + rlon = math.radians(rlon) + lat = math.radians(lat) + lon = math.radians(lon) + + rx, ry, rz = toECEF(rlat, rlon, rdep) + x, y, z = toECEF(lat, lon, dep) + + ox = x - rx + oy = y - ry + oz = z - rz + + # North. + n = -math.sin(rlat) * math.cos(rlon) * ox - math.sin(rlat) * math.sin(rlon) * oy + math.cos(rlat) * oz + + # East. + e = -math.sin(rlon) * ox + math.cos(rlon) * oy + + # Down. + d = -math.cos(rlat) * math.cos(rlon) * ox - math.cos(rlat) * math.sin(rlon) * oy - math.sin(rlat) * oz + + return n, e, d + +def toECEF(lat, lon, dep): + + #Semi-major axis. + c_wgs84_a = 6378137.0 + #First eccentricity squared. + c_wgs84_e2 = 0.00669437999013 + + rn = c_wgs84_a / math.sqrt(1 - c_wgs84_e2 * (math.sin(lat) * math.sin(lat))) + + x = (rn + dep) * math.cos(lat) * math.cos(lon) + y = (rn + dep) * math.cos(lat) * math.sin(lon) + z = (((1.0 - c_wgs84_e2) * rn) + dep) * math.sin(lat) + + return x, y, z + +# Create target grid and interpolation limits +def targetGrid(olat, olon, Blat, Blon, radius): + + # Calculate step (for ~240000 points) + x_interval = Blat - olat + y_interval = Blon - olon + step = (x_interval * y_interval / 62500) ** 0.5 + + x_step = step if x_interval > 0 else -step + y_step = step if y_interval > 0 else -step + + # Create grid and convert to NED + target_points = [] + for x in range(0, int(abs(x_interval / x_step))+1): + for y in range(0, int(abs(y_interval / y_step))+1): + for z in range(-15, 5, 5): + x_m, y_m, z_m = WGS84toNED(olat, olon, 0, olat + x*x_step, olon + y*y_step, z) + target_points.append([x_m, y_m, z_m]) + + # Find interpolation limits + x_limit, y_limit, z_m = WGS84toNED(olat, olon, 0, olat + x_interval, olon + y_interval, 0) + if x_interval > 0: + x_min = -radius + x_max = x_limit + radius + else: + x_min = x_limit - radius + x_max = radius + if y_interval > 0: + y_min = -radius + y_max = y_limit + radius + else: + y_min = y_limit - radius + y_max = radius + + x_limits = [x_min, x_max] + y_limits = [y_min, y_max] + + return target_points, x_limits, y_limits + +# Input data. +def getInputData(input_file_path, time): + + #Get data of specific time + time = str(time) + time = "0"*(5-len(time)) + time + + #Get data points + input_file = h5py.File(input_file_path, 'r') + lat = list(input_file['Grid/Latitude']) + lon = list(input_file['Grid/Longitude']) + dep = list(input_file['Grid/VerticalZ/Vertical_' + time]) + temp_data_raw = list(input_file['Results/temperature/temperature_' + time]) + sal_data_raw = list(input_file['Results/salinity/salinity_' + time]) + valid_points = list(input_file['Grid/OpenPoints/OpenPoints_' + time]) + + #Align grid + lat = lat[0][1:] + lon = [lon[i][0] for i in range(1, len(lon))] + dep = dep[1:] + + #Filter valid points + data_points = [] + temp_data = [] + sal_data = [] + for x in range(0, len(lat)): + for y in range(0, len(lon)): + for z in range(0, len(dep)): + if valid_points[z][y][x] == 1: + data_points.append([lat[x], lon[y], dep[z][y][x]]) + temp_data.append(temp_data_raw[z][y][x]) + sal_data.append(sal_data_raw[z][y][x]) + + return data_points, temp_data, sal_data + +# Output file +def writeToFile(target_points, data, parameter, o_reference, time): + + # Create file + output_file = open(args.dstpath + parameter.lower() + "_" + str(time) + "-" + args.location.lower() + ".ini", "w") + + # Get licence from this script + self_file = open("./programs/scripts/dune-create-data-file.py") + header = False + for line in self_file.readlines(): + line = line.strip() + if line.startswith('# Author: '): + break + if line.startswith('##') and not header: + header = True + output_file.write(line + '\n') + elif header: + output_file.write(line + '\n') + + # Write preamble (parameter, origin reference and data field) + PREAMBLE = ['\n', + '['+parameter+']\n', + '\n', + 'Latitude (degrees) = ' + str(o_reference[0]) + '\n', + 'Longitude (degrees) = ' + str(o_reference[1]) + '\n', + '\n', + 'Data = '] + for i in range(0, len(PREAMBLE)): + output_file.write(PREAMBLE[i]) + + # Write data + for i in range(0, len(data)): + if not math.isnan(data[i]): + aux = ['{0:4.2f}'.format(j) for j in target_points[i]] + aux.append('{0:4.2f}'.format(data[i])) + aux = ' '.join(aux) + if not i == 0: + aux = aux.rjust(len(PREAMBLE[-1]) + len(aux)) + output_file.write(aux + + (",\n" if i < len(data) - 1 else "\n")) + +########################################## Parse command line arguments ############################################### +parser = argparse.ArgumentParser( + description="This script processes hdf5 temperature and salinity data.") +parser.add_argument('srcpath', metavar='', + help="Path to source hdf5 data file") +parser.add_argument('oLat', metavar='', type=float, + help="Latitude of lower right corner of interpolation area") +parser.add_argument('oLon', metavar='', type=float, + help="Longitude of lower right corner of interpolation area") +parser.add_argument('urLat', metavar='', type=float, + help="Latitude of upper left corner of interpolation area") +parser.add_argument('urLon', metavar='', type=float, + help="Longitude of upper left corner of interpolation area") + +parser.add_argument('--dstpath', metavar='', default="./etc/simulation/", + help="Destination path for file output. Default: ../../etc/simulation/") +parser.add_argument('--location', metavar='', default="data", + help="Desired name of location (Ex: apdl). Default: \"data\"") +parser.add_argument('--times', metavar='', default=1, type=int, + help="Number of files to produce, corresponding to data from different times.") +parser.add_argument('--radius', metavar='', default=60000, type=float, + help="Range of source data beyond interpolation area. Default: 60000 m") +parser.add_argument('--plot', action="store_true", default=0, + help="Plots the last temperature data set. Requires matplotlib package.") +args = parser.parse_args() + +################################################# Create target grid ################################################## +print "Starting..." +target_points, x_limits, y_limits = targetGrid(args.oLat, args.oLon, args.urLat, args.urLon, args.radius) + +########################################## Interpolation and write to file ############################################ + +print "Converting..." +target_points_arr = np.array(target_points) +start_time = time.time() +for i in range(0, args.times): + + #Clear + data_points = [] + temp_data_input = [] + sal_data_input = [] + + #Get temperature and salinity data + data_points_deg, temp_data, sal_data = getInputData(args.srcpath, i+1) + + #Trim points for interpolation, using interpolation radius + for idx in range(0, len(data_points_deg)): + n, e, d = WGS84toNED(args.oLat, args.oLon, 0, + data_points_deg[idx][0], + data_points_deg[idx][1], + data_points_deg[idx][2]) + if (x_limits[0] < n < x_limits[1] and + y_limits[0] < e < y_limits[1]): + data_points.append([n, e, d]) + temp_data_input.append(temp_data[idx]) + sal_data_input.append(sal_data[idx]) + + #Interpolate + data_points_arr = np.array(data_points) + temp_values_arr = np.array(temp_data_input) + sal_values_arr = np.array(sal_data_input) + + temp_interpol = griddata(data_points_arr, temp_values_arr, target_points, method='linear') + sal_interpol = griddata(data_points_arr, sal_values_arr, target_points, method='linear') + + #Write to file + writeToFile(target_points, temp_interpol, "Temperature", (args.oLat, args.oLon), i) + writeToFile(target_points, sal_interpol, "Salinity", (args.oLat, args.oLon), i) + print "Completed: " + str((i+1)*100/args.times) + "%" + print "Time remaining: " + '{0:2.1f}'.format((time.time()-start_time)*(args.times - (i + 1))/60) + "m" + start_time = time.time() + +##################################################### Plot data ####################################################### +if args.plot: + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d import Axes3D + x_p = [] + y_p = [] + z_p = [] + for point in target_points: + x_p.append(point[0]) + y_p.append(point[1]) + z_p.append(-point[2]) + + fig = plt.figure() + ax = plt.axes(projection='3d') + p = ax.scatter3D(x_p, y_p, z_p, c=temp_interpol, s=0.2) + fig.colorbar(p) + plt.show() diff --git a/src/DUNE/Algorithms.hpp b/src/DUNE/Algorithms.hpp index 2bbfd5b902..181ffc68db 100644 --- a/src/DUNE/Algorithms.hpp +++ b/src/DUNE/Algorithms.hpp @@ -44,5 +44,7 @@ namespace DUNE #include #include #include +#include +#include #endif diff --git a/src/DUNE/Algorithms/OcTree/Bounds.hpp b/src/DUNE/Algorithms/OcTree/Bounds.hpp new file mode 100644 index 0000000000..735c27c505 --- /dev/null +++ b/src/DUNE/Algorithms/OcTree/Bounds.hpp @@ -0,0 +1,231 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +#ifndef SIMULATORS_ALGORITHMS_OC_TREE_BOUNDS_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_OC_TREE_BOUNDS_HPP_INCLUDED_ + +// ISO C++ headers. +#include +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "Point.hpp" + +namespace DUNE +{ + namespace Algorithms + { + namespace Oc + { + struct Bounds + { + double min_x, max_x, min_y, max_y, min_z, max_z; + + Bounds(const Point& p, double r): + min_x(p.x - r), max_x(p.x + r), + min_y(p.y - r), max_y(p.y + r), + min_z(p.z - r), max_z(p.z + r) + { } + + Bounds(const Point& p, double rx, double ry, double rz): + min_x(p.x - rx), max_x(p.x + rx), + min_y(p.y - ry), max_y(p.y + ry), + min_z(p.z - rz), max_z(p.z + rz) + { } + + Bounds(const Point& p): + min_x(p.x), max_x(p.x), + min_y(p.y), max_y(p.y), + min_z(p.z), max_z(p.z) + { } + + Bounds(const Bounds& parent, const Point& lim, int dir) + { + switch (dir) + { + case DIR_UNW: + min_x = lim.x; + max_x = parent.max_x; + min_y = parent.min_y; + max_y = lim.y; + min_z = lim.z; + max_z = parent.max_z; + break; + case DIR_UNE: + min_x = lim.x; + max_x = parent.max_x; + min_y = lim.y; + max_y = parent.max_y; + min_z = lim.z; + max_z = parent.max_z; + break; + case DIR_USW: + min_x = parent.min_x; + max_x = lim.x; + min_y = parent.min_y; + max_y = lim.y; + min_z = lim.z; + max_z = parent.max_z; + break; + case DIR_USE: + min_x = parent.min_x; + max_x = lim.x; + min_y = lim.y; + max_y = parent.max_y; + min_z = lim.z; + max_z = parent.max_z; + break; + case DIR_DNW: + min_x = lim.x; + max_x = parent.max_x; + min_y = parent.min_y; + max_y = lim.y; + min_z = parent.min_z; + max_z = lim.z; + break; + case DIR_DNE: + min_x = lim.x; + max_x = parent.max_x; + min_y = lim.y; + max_y = parent.max_y; + min_z = parent.min_z; + max_z = lim.z; + break; + case DIR_DSW: + min_x = parent.min_x; + max_x = lim.x; + min_y = parent.min_y; + max_y = lim.y; + min_z = parent.min_z; + max_z = lim.z; + break; + case DIR_DSE: + default: + min_x = parent.min_x; + max_x = lim.x; + min_y = lim.y; + max_y = parent.max_y; + min_z = parent.min_z; + max_z = lim.z; + break; + } + } + + Bounds + octant(int dir) const + { + return Bounds(*this, midpoint(), dir); + } + + template + std::pair + octant(const T& p) const + { + Point lim = midpoint(); + int q = lim.direction(p); + return std::pair(q, Bounds(*this, lim, q)); + } + + bool + intersects(const Bounds& other) const + { + if (min_x > other.max_x || other.min_x > max_x) + return false; + if (min_y > other.max_y || other.min_y > max_y) + return false; + if (min_z > other.max_z || other.min_z > max_z) + return false; + return true; + } + + template + void + cover(const T& p) + { + if (p.x < min_x) + min_x = p.x; + else if (p.x > max_x) + max_x = p.x; + + if (p.y < min_y) + min_y = p.y; + else if (p.y > max_y) + max_y = p.y; + + if (p.z < min_z) + min_z = p.z; + else if (p.z > max_z) + max_z = p.z; + } + + template + bool + contains(const T& p) const + { + return p.x >= min_x && p.x <= max_x && + p.y >= min_y && p.y <= max_y && + p.z >= min_z && p.z <= max_z; + } + + Point + midpoint() const + { + return Point(0.5 * (min_x + max_x), 0.5 * (min_y + max_y), 0.5 * (min_z + max_z)); + } + + double + width() const + { + return max_x - min_x; + } + + double + lenght() const + { + return max_y - min_y; + } + + double + height() const + { + return max_z - min_z; + } + }; + + std::ostream& + operator<<(std::ostream& os, const Bounds& b); + } + } +} + +#endif diff --git a/src/DUNE/Algorithms/OcTree/OcTree.cpp b/src/DUNE/Algorithms/OcTree/OcTree.cpp new file mode 100644 index 0000000000..2b454a7bbc --- /dev/null +++ b/src/DUNE/Algorithms/OcTree/OcTree.cpp @@ -0,0 +1,304 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "OcTree.hpp" + +namespace DUNE +{ + namespace Algorithms + { + namespace Oc + { + // The actual implementation + struct Node + { + union + { + OcTree::Item item; + Node* children[8]; + } m_data; + + bool m_leaf; + + Node(const OcTree::Item& item) + { + m_data.item = item; + m_leaf = true; + } + + ~Node(void) + { + if (!m_leaf) + { + for (int i = 0; i < 8; ++i) + { + if (m_data.children[i]) + delete m_data.children[i]; + } + } + } + + void + insert(const OcTree::Item& item, Bounds& b) + { + if (m_leaf) + { + m_leaf = false; + OcTree::Item prev_item = m_data.item; + std::memset(m_data.children, 0, sizeof(m_data.children)); + insert(prev_item, b); // Split + } + std::pair bq = b.octant(item); + Node** c = m_data.children + bq.first; + if (*c) + (*c)->insert(item, bq.second); // Branch + else + *c = new Node(item); // New leaf + } + + bool + isLeaf(void) const + { + return m_leaf; + } + + bool + remove(const Bounds& area, const Bounds& b) + { + if (m_leaf) + return area.contains(m_data.item); + + int cdel = 0; + + for (int i = 0; i < 8; ++i) + { + Node** c = m_data.children + i; + + if (!*c) + continue; + + Bounds cb = b.octant(i); + + if (area.intersects(cb) && (*c)->remove(area, cb)) + { + delete *c; + * c = 0; + ++cdel; + } + } + return cdel == 8; // true if all sub-nodes were removed + } + + void + iterate(OcTree::Iteration& iter, const Bounds& area, const Bounds& b) const + { + if (m_leaf && b.contains(m_data.item)) + { + iter.process(m_data.item); + } + else + for (int i = 0; i < 8; ++i) + { + if (m_data.children[i]) + { + Bounds cb = b.octant(i); + if (area.intersects(cb)) + m_data.children[i]->iterate(iter, area, cb); + } + } + } + }; + + OcTree::OcTree(const Bounds& bounds): + m_bounds(bounds), m_root(0) + { } + + OcTree::~OcTree() + { + clear(); + } + + void + OcTree::iterate(Iteration& iter) const + { + iterate(iter, m_bounds); + } + + void + OcTree::iterate(Iteration& iter, const Bounds& area) const + { + if (!m_root) + return; + + m_root->iterate(iter, area, m_bounds); + } + + void + OcTree::clear() + { + if (m_root) + { + delete m_root; + m_root = 0; + } + } + + void + OcTree::remove(const Bounds& area) + { + if (m_root && m_root->remove(area, m_bounds)) + clear(); // entire contents were deleted + } + + bool + OcTree::insert(const OcTree::Item& item) + { + if (!m_bounds.contains(item)) + return false; + + if (!m_root) + m_root = new Node(item); + else + m_root->insert(item, m_bounds); + + return true; + } + + bool + OcTree::search(const Bounds& search_area, std::vector& v) const + { + class Search: public OcTree::Iteration + { + public: + Search(std::vector& vector): m_vector(vector) { } + + ~Search(){ } + + void + process(const OcTree::Item& item) + { + m_vector.push_back(item); + } + + private: + std::vector& m_vector; + }; + + v.clear(); + Search iter(v); + + if (m_root) + m_root->iterate(iter, search_area, m_bounds); + + return v.size() != 0; + } + + uint32_t + OcTree::size(const Bounds& area) const + { + class count: public OcTree::Iteration + { + public: + count(): m_elems(0) { } + + ~count(){ } + + void + process(const OcTree::Item& item) + { + (void)item; + ++m_elems; + } + + uint32_t + result(void) + { + return m_elems; + } + + private: + uint32_t m_elems; + }; + + count iter; + iterate(iter, area); + return iter.result(); + } + + class Dump: public OcTree::Iteration + { + public: + Dump(std::ostream& stream): + m_stream(stream) + { } + + ~Dump(void) + { } + + void + process(const OcTree::Item& item) + { + m_stream << item.x << ' ' << item.y << ' ' << item.z; + m_stream << ' ' << item.value; + m_stream << std::endl; + } + + private: + std::ostream& m_stream; + }; + + std::ostream& + operator<<(std::ostream& os, const OcTree& tree) + { + Dump iter(os); + tree.iterate(iter); + return os; + } + + std::ostream& + operator<<(std::ostream& os, const Bounds& b) + { + return + os << std::fixed << std::setprecision(2) << + b.min_x << ' ' << b.max_x << " | " << + b.min_y << ' ' << b.max_y << " | " << + b.min_z << ' ' << b.max_z << " | " << + b.width() << " x " << b.lenght() << " x " << b.height(); + } + } + } +} diff --git a/src/Simulators/Environment/QuadTree.hpp b/src/DUNE/Algorithms/OcTree/OcTree.hpp similarity index 59% rename from src/Simulators/Environment/QuadTree.hpp rename to src/DUNE/Algorithms/OcTree/OcTree.hpp index e7a0e16393..56edd64a23 100644 --- a/src/Simulators/Environment/QuadTree.hpp +++ b/src/DUNE/Algorithms/OcTree/OcTree.hpp @@ -27,8 +27,8 @@ // Author: Eduardo Marques * //*************************************************************************** -#ifndef SIMULATORS_ENVIRONMENT_QUAD_TREE_HPP_INCLUDED_ -#define SIMULATORS_ENVIRONMENT_QUAD_TREE_HPP_INCLUDED_ +#ifndef SIMULATORS_ALGORITHMS_OC_TREE_OC_TREE_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_OC_TREE_OC_TREE_HPP_INCLUDED_ // ISO C++ 98 headers. #include @@ -42,76 +42,79 @@ #include "Point.hpp" #include "Bounds.hpp" -namespace Simulators +namespace DUNE { - namespace Environment + namespace Algorithms { - //! Forward declaration. - struct Node; - - //! "Quad-tree" structure used to index spatial data in two dimensions. - class QuadTree + namespace Oc { - public: - //! Item datum. - struct Item - { - double x, y, value; - }; + //! Forward declaration. + struct Node; - //! Iteration handle. - class Iteration + //! "Oc-tree" structure used to index spatial data in two dimensions. + class OcTree { public: - virtual void - process(const Item& item) = 0; - - virtual - ~Iteration(){ } + //! Item datum. + struct Item + { + double x, y, z, value; + }; + + //! Iteration handle. + class Iteration + { + public: + virtual void + process(const Item& item) = 0; + + virtual + ~Iteration(){ } + }; + + //! Constructor. + OcTree(const Bounds& bounds); + + ~OcTree(); + + //! Insert an item. + //! Returns 'false' if item is out-of-bounds. + bool + insert(const Item& item); + + //! Iterate tree. + void + iterate(Iteration& iteration) const; + + //! Iterate tree over a given area. + void + iterate(Iteration& iteration, const Bounds& area) const; + + //! Clear entire tree. + void + clear(void); + + //! Remove items in a given area. + void + remove(const Bounds& area); + + //! Search for items in a given area. + //! If any are found returns 'true' and adds results to 'item' vector. + bool + search(const Bounds& area, std::vector& items) const; + + //! Get number of elements in a given area. + uint32_t + size(const Bounds& area) const; + + private: + Bounds m_bounds; //!< Bounds. + Node* m_root; //!< Node handle providing actual implementation. }; - //! Constructor. - QuadTree(const Bounds& bounds); - - ~QuadTree(); - - //! Insert an item. - //! Returns 'false' if item is out-of-bounds. - bool - insert(const Item& item); - - //! Iterate tree. - void - iterate(Iteration& iteration) const; - - //! Iterate tree over a given area. - void - iterate(Iteration& iteration, const Bounds& area) const; - - //! Clear entire tree. - void - clear(void); - - //! Remove items in a given area. - void - remove(const Bounds& area); - - //! Search for items in a given area. - //! If any are found returns 'true' and adds results to 'item' vector. - bool - search(const Bounds& area, std::vector& items) const; - - //! Get number of elements in a given area. - uint32_t - size(const Bounds& area) const; - - private: - Bounds m_bounds; //!< Bounds. - Node* m_root; //!< Node handle providing actual implementation. - }; - - std::ostream& - operator<<(std::ostream& os, const QuadTree& tree); + std::ostream& + operator<<(std::ostream& os, const OcTree& tree); + } } } diff --git a/src/DUNE/Algorithms/OcTree/Point.hpp b/src/DUNE/Algorithms/OcTree/Point.hpp new file mode 100644 index 0000000000..3c3be5128d --- /dev/null +++ b/src/DUNE/Algorithms/OcTree/Point.hpp @@ -0,0 +1,89 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +#ifndef SIMULATORS_ALGORITHMS_OC_TREE_POINT_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_OC_TREE_POINT_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +namespace DUNE +{ + namespace Algorithms + { + namespace Oc + { + //! Directions for search/insertion + enum + { + DIR_UNW, DIR_UNE, DIR_USW, DIR_USE, + DIR_DNW, DIR_DNE, DIR_DSW, DIR_DSE + }; + + //! Point structure. + struct Point + { + double x, y, z; + + Point(double x_, double y_, double z_): + x(x_), y(y_), z(z_) { } + + double + distance(const Point& other) const + { + double dx = x - other.x; + double dy = y - other.y; + double dz = z - other.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); + } + + template + int + direction(const T& other) const + { + double dx = other.x - x; + double dy = other.y - y; + double dz = other.z - z; + + uint8_t dir = dz >= 0 ? DIR_UNW : DIR_DNW; // U/D selection + dir += dx >= 0 ? 0 : 2; // N/S selection + dir += dy >= 0 ? 1 : 0; // E/W selection + + return dir; + } + }; + } + } +} + +#endif diff --git a/src/DUNE/Algorithms/QuadTree/Bounds.hpp b/src/DUNE/Algorithms/QuadTree/Bounds.hpp new file mode 100644 index 0000000000..ebaafe4104 --- /dev/null +++ b/src/DUNE/Algorithms/QuadTree/Bounds.hpp @@ -0,0 +1,166 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +#ifndef SIMULATORS_ALGORITHMS_QUAD_TREE_BOUNDS_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_QUAD_TREE_BOUNDS_HPP_INCLUDED_ + +// ISO C++ headers. +#include +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "Point.hpp" + +namespace DUNE +{ + namespace Algorithms + { + namespace Quad + { + struct Bounds + { + double min_x, max_x, min_y, max_y; + + Bounds(const Point& p, double r): + min_x(p.x - r), max_x(p.x + r), min_y(p.y - r), max_y(p.y + r) + { } + + Bounds(const Point& p): + min_x(p.x), max_x(p.x), min_y(p.y), max_y(p.y) + { } + + Bounds(const Bounds& parent, const Point& lim, int dir) + { + switch (dir) + { + case DIR_NW: + min_x = lim.x; + max_x = parent.max_x; + min_y = parent.min_y; + max_y = lim.y; + break; + case DIR_NE: + min_x = lim.x; + max_x = parent.max_x; + min_y = lim.y; + max_y = parent.max_y; + break; + case DIR_SW: + min_x = parent.min_x; + max_x = lim.x; + min_y = parent.min_y; + max_y = lim.y; + break; + case DIR_SE: + default: + min_x = parent.min_x; + max_x = lim.x; + min_y = lim.y; + max_y = parent.max_y; + break; + } + } + + Bounds + quadrant(int dir) const + { + return Bounds(*this, midpoint(), dir); + } + + template + std::pair + quadrant(const T& p) const + { + Point lim = midpoint(); + int q = lim.direction(p); + return std::pair(q, Bounds(*this, lim, q)); + } + + bool + intersects(const Bounds& other) const + { + if (min_x > other.max_x || other.min_x > max_x) + return false; + if (min_y > other.max_y || other.min_y > max_y) + return false; + return true; + } + + template + void + cover(const T& p) + { + if (p.x < min_x) + min_x = p.x; + else if (p.x > max_x) + max_x = p.x; + + if (p.y < min_y) + min_y = p.y; + else if (p.y > max_y) + max_y = p.y; + } + + template + bool + contains(const T& p) const + { + return p.x >= min_x && p.x <= max_x && p.y >= min_y && p.y <= max_y; + } + + Point + midpoint() const + { + return Point(0.5 * (min_x + max_x), 0.5 * (min_y + max_y)); + } + + double + width() const + { + return max_x - min_x; + } + + double + height() const + { + return max_y - min_y; + } + }; + + std::ostream& + operator<<(std::ostream& os, const Bounds& b); + } + } +} + +#endif diff --git a/src/DUNE/Algorithms/QuadTree/Point.hpp b/src/DUNE/Algorithms/QuadTree/Point.hpp new file mode 100644 index 0000000000..d25c38fced --- /dev/null +++ b/src/DUNE/Algorithms/QuadTree/Point.hpp @@ -0,0 +1,87 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +#ifndef SIMULATORS_ALGORITHMS_QUAD_TREE_POINT_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_QUAD_TREE_POINT_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +namespace DUNE +{ + namespace Algorithms + { + namespace Quad + { + //! Directions for search/insertion + enum + { + DIR_NW, DIR_NE, DIR_SW, DIR_SE + }; + + //! Point structure. + struct Point + { + double x, y; + + Point(double x_, double y_): + x(x_), y(y_) { } + + double + distance(const Point& other) const + { + double dx = x - other.x; + double dy = y - other.y; + return std::sqrt(dx * dx + dy * dy); + } + + template + int + direction(const T& other) const + { + double dx = other.x - x; + double dy = other.y - y; + + int dir = dx >= 0 ? DIR_NW : DIR_SW; + + if (dy > 0) + dir++; // other is East of this + + return dir; + } + }; + } + } +} + +#endif diff --git a/src/DUNE/Algorithms/QuadTree/QuadTree.cpp b/src/DUNE/Algorithms/QuadTree/QuadTree.cpp new file mode 100644 index 0000000000..27fc7d30da --- /dev/null +++ b/src/DUNE/Algorithms/QuadTree/QuadTree.cpp @@ -0,0 +1,299 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +// ISO C++ 98 headers. +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "QuadTree.hpp" + +namespace DUNE +{ + namespace Algorithms + { + namespace Quad + { + // The actual implementation + struct Node + { + union + { + QuadTree::Item item; + Node* children[4]; + } m_data; + + bool m_leaf; + + Node(const QuadTree::Item& item) + { + m_data.item = item; + m_leaf = true; + } + + ~Node(void) + { + if (!m_leaf) + { + for (int i = 0; i < 4; ++i) + { + if (m_data.children[i]) + delete m_data.children[i]; + } + } + } + + void + insert(const QuadTree::Item& item, Bounds& b) + { + if (m_leaf) + { + m_leaf = false; + QuadTree::Item prev_item = m_data.item; + std::memset(m_data.children, 0, sizeof(m_data.children)); + insert(prev_item, b); // Split + } + std::pair bq = b.quadrant(item); + Node** c = m_data.children + bq.first; + if (*c) + (*c)->insert(item, bq.second); // Branch + else + *c = new Node(item); // New leaf + } + + bool + isLeaf(void) const + { + return m_leaf; + } + + bool + remove(const Bounds& area, const Bounds& b) + { + if (m_leaf) + return area.contains(m_data.item); + + int cdel = 0; + + for (int i = 0; i < 4; ++i) + { + Node** c = m_data.children + i; + + if (!*c) + continue; + + Bounds cb = b.quadrant(i); + + if (area.intersects(cb) && (*c)->remove(area, cb)) + { + delete *c; + * c = 0; + ++cdel; + } + } + return cdel == 4; // true if all sub-nodes were removed + } + + void + iterate(QuadTree::Iteration& iter, const Bounds& area, const Bounds& b) const + { + if (m_leaf && b.contains(m_data.item)) + iter.process(m_data.item); + else + for (int i = 0; i < 4; ++i) + { + if (m_data.children[i]) + { + Bounds cb = b.quadrant(i); + if (area.intersects(cb)) + m_data.children[i]->iterate(iter, area, cb); + } + } + } + }; + + QuadTree::QuadTree(const Bounds& bounds): + m_bounds(bounds), m_root(0) + { } + + QuadTree::~QuadTree() + { + clear(); + } + + void + QuadTree::iterate(Iteration& iter) const + { + iterate(iter, m_bounds); + } + + void + QuadTree::iterate(Iteration& iter, const Bounds& area) const + { + if (!m_root) + return; + + m_root->iterate(iter, area, m_bounds); + } + + void + QuadTree::clear() + { + if (m_root) + { + delete m_root; + m_root = 0; + } + } + + void + QuadTree::remove(const Bounds& area) + { + if (m_root && m_root->remove(area, m_bounds)) + clear(); // entire contents were deleted + } + + bool + QuadTree::insert(const QuadTree::Item& item) + { + if (!m_bounds.contains(item)) + return false; + + if (!m_root) + m_root = new Node(item); + else + m_root->insert(item, m_bounds); + + return true; + } + + bool + QuadTree::search(const Bounds& search_area, std::vector& v) const + { + class Search: public QuadTree::Iteration + { + public: + Search(std::vector& vector): m_vector(vector) { } + + ~Search(){ } + + void + process(const QuadTree::Item& item) + { + m_vector.push_back(item); + } + + private: + std::vector& m_vector; + }; + + v.clear(); + Search iter(v); + + if (m_root) + m_root->iterate(iter, search_area, m_bounds); + + return v.size() != 0; + } + + uint32_t + QuadTree::size(const Bounds& area) const + { + class count: public QuadTree::Iteration + { + public: + count(): m_elems(0) { } + + ~count(){ } + + void + process(const QuadTree::Item& item) + { + (void)item; + ++m_elems; + } + + uint32_t + result(void) + { + return m_elems; + } + + private: + uint32_t m_elems; + }; + + count iter; + iterate(iter, area); + return iter.result(); + } + + class Dump: public QuadTree::Iteration + { + public: + Dump(std::ostream& stream): + m_stream(stream) + { } + + ~Dump(void) + { } + + void + process(const QuadTree::Item& item) + { + m_stream << item.x << ' ' << item.y << ' ' << item.value << std::endl; + } + + private: + std::ostream& m_stream; + }; + + std::ostream& + operator<<(std::ostream& os, const QuadTree& tree) + { + Dump iter(os); + tree.iterate(iter); + return os; + } + + std::ostream& + operator<<(std::ostream& os, const Bounds& b) + { + return + os << std::fixed << std::setprecision(2) << + b.min_x << ' ' << b.max_x << " | " << + b.min_y << ' ' << b.max_y << " | " << + b.width() << " x " << b.height(); + } + } + } +} diff --git a/src/DUNE/Algorithms/QuadTree/QuadTree.hpp b/src/DUNE/Algorithms/QuadTree/QuadTree.hpp new file mode 100644 index 0000000000..7a21a139d1 --- /dev/null +++ b/src/DUNE/Algorithms/QuadTree/QuadTree.hpp @@ -0,0 +1,121 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Eduardo Marques * +//*************************************************************************** + +#ifndef SIMULATORS_ALGORITHMS_QUAD_TREE_QUAD_TREE_HPP_INCLUDED_ +#define SIMULATORS_ALGORITHMS_QUAD_TREE_QUAD_TREE_HPP_INCLUDED_ + +// ISO C++ 98 headers. +#include +#include +#include + +// DUNE headers. +#include + +// Local headers. +#include "Point.hpp" +#include "Bounds.hpp" + +namespace DUNE +{ + namespace Algorithms + { + namespace Quad + { + //! Forward declaration. + struct Node; + + //! "Quad-tree" structure used to index spatial data in two dimensions. + class QuadTree + { + public: + //! Item datum. + struct Item + { + double x, y, value; + }; + + //! Iteration handle. + class Iteration + { + public: + virtual void + process(const Item& item) = 0; + + virtual + ~Iteration(){ } + }; + + //! Constructor. + QuadTree(const Bounds& bounds); + + ~QuadTree(); + + //! Insert an item. + //! Returns 'false' if item is out-of-bounds. + bool + insert(const Item& item); + + //! Iterate tree. + void + iterate(Iteration& iteration) const; + + //! Iterate tree over a given area. + void + iterate(Iteration& iteration, const Bounds& area) const; + + //! Clear entire tree. + void + clear(void); + + //! Remove items in a given area. + void + remove(const Bounds& area); + + //! Search for items in a given area. + //! If any are found returns 'true' and adds results to 'item' vector. + bool + search(const Bounds& area, std::vector& items) const; + + //! Get number of elements in a given area. + uint32_t + size(const Bounds& area) const; + + private: + Bounds m_bounds; //!< Bounds. + Node* m_root; //!< Node handle providing actual implementation. + }; + + std::ostream& + operator<<(std::ostream& os, const QuadTree& tree); + } + } +} + +#endif diff --git a/src/DUNE/Algorithms/UNESCO1983.cpp b/src/DUNE/Algorithms/UNESCO1983.cpp index d818b00098..452887bd68 100644 --- a/src/DUNE/Algorithms/UNESCO1983.cpp +++ b/src/DUNE/Algorithms/UNESCO1983.cpp @@ -62,7 +62,7 @@ namespace DUNE double cr = c / 4.2914; // Equation is not defined for conductivity ratios below 5e-4. - if (cr <= 5e-4) + if (cr <= 5e-4 || (t<-2 || t>35)) return -1.0; double dt = t - 15; @@ -75,10 +75,7 @@ namespace DUNE double rt35 = (((1.0031e-9 * t - 6.9698e-7) * t + 1.104259e-4) * t + 2.00564e-2) * t + 0.6766097; double rt = cr / (rt35 * (1.0 + cxp / (bxt + axt * cr))); rt = std::sqrt(std::fabs(rt)); - double salinity = (((((2.7081 * rt - 7.0261) * rt + 14.0941) * rt + 25.3851) * rt - 0.1692) - * rt + 0.0080 + (dt / (1.0 + 0.0162 * dt)) - * (((((-0.0144 * rt + 0.0636) * rt - 0.0375) * rt - 0.0066) * rt - 0.0056) - * rt + 0.0005)); + double salinity = salinity_poli(rt, dt); // Check if result is in the validity range of PSS-78. if ((salinity < 2) || (salinity > 42)) @@ -87,6 +84,47 @@ namespace DUNE return salinity; } + double + UNESCO1983::computeConductivity(double s, double p, double t) + { + // Equation is not defined for salinity below 0.02. + if (s <= 0.02 || (t<-2 || t>35)) + return -1.0; + + // Convert bar to dbar. + p *= 10; + + // Auxiliars + double dt = t - 15; + double axt = -3.107e-3 * t + 0.4215; + // Polynomial corresponds to B3 and B4 constants (Lewis 1980). + double bxt = (4.464e-4 * t + 3.426e-2) * t + 1.0; + // Polynomial corresponds to A1-A3 constants (Lewis 1980). + double cxp = ((3.989e-15 * p - 6.370e-10) * p + 2.070e-5) * p; + // Variation with temperature. + double rt35 = (((1.0031e-9 * t - 6.9698e-7) * t + 1.104259e-4) * t + 2.00564e-2) * t + 0.6766097; + + //Newton Raphson method + // Temperature ratio sqrt. Initial guess + double rt = std::sqrt(std::abs(s / 35)); + // Salinity initial aproximation + double s_aprox = salinity_poli(rt, dt); + for(unsigned i=0; std::abs(s - s_aprox) > 1e-5; ++i) + { + rt = rt + (s - s_aprox)/salinity_poli_deriv(rt, dt); + s_aprox = salinity_poli(rt, dt); + } + + // Solve quadratic equation + rt = rt*rt; + double cr = bxt-axt*rt35*rt; + cr = std::sqrt(std::abs(cr*cr + 4*rt35*rt*axt*(bxt+cxp))); + cr = cr - (bxt - axt*rt35*rt); + cr = 0.5*cr/axt; + + return cr*4.2914; + } + double UNESCO1983::computeSoundSpeed(double s, double p, double t) { diff --git a/src/DUNE/Algorithms/UNESCO1983.hpp b/src/DUNE/Algorithms/UNESCO1983.hpp index 568369c749..5e00890b54 100644 --- a/src/DUNE/Algorithms/UNESCO1983.hpp +++ b/src/DUNE/Algorithms/UNESCO1983.hpp @@ -64,6 +64,15 @@ namespace DUNE static double computeSalinity(double conductivity, double pressure, double temperature); + //! Compute conductivity. + //! @param salinity (PSU). + //! @param pressure (bar). + //! @param temperature (ºC). + //! @return conductivity (S/m) or a negative number if the + //! computation is not reliable. + static double + computeConductivity(double s, double p, double t); + //! Compute sound speed. //! @param salinity (PSU) //! @param pressure (bar). @@ -71,6 +80,37 @@ namespace DUNE //! @return sound speed (m/s). static double computeSoundSpeed(double salinity, double pressure, double temperature); + + private: + //! Salinity polinomial as defined in: + //! 'Algorithms for computation of fundamental + //! properties of seawater' (UNESCO 1983) + //! @param rt sqrt of temperature ratio. + //! @param dt (ºC) corrected temperature (t-15). + //! @return salinity (PSU). + static double + salinity_poli(double rt, double dt) + { + return (((((2.7081 * rt - 7.0261) * rt + 14.0941) * rt + 25.3851) * rt - 0.1692) + * rt + 0.0080 + (dt / (1.0 + 0.0162 * dt)) + * (((((-0.0144 * rt + 0.0636) * rt - 0.0375) * rt - 0.0066) * rt - 0.0056) + * rt + 0.0005)); + } + + //! Salinity polinomial derivative: + //! 'Algorithms for computation of fundamental + //! properties of seawater' (UNESCO 1983) + //! @param rt sqrt of temperature ratio. + //! @param dt (ºC) corrected temperature (t-15). + //! @return polinomial derivative with respect to rt. + static double + salinity_poli_deriv(double rt, double dt) + { + return (((((13.5405 * rt - 28.1044) * rt + 42.2823) * rt + 50.7702) * rt - 0.1692) + + (dt / (1.0 + 0.0162 * dt)) + * ((((-0.072 * rt + 0.2544) * rt - 0.1125) * rt - 0.0132) * rt - 0.0056)); + } + }; } } diff --git a/src/DUNE/Parsers/HDF5Reader.cpp b/src/DUNE/Parsers/HDF5Reader.cpp new file mode 100644 index 0000000000..6bb793d46d --- /dev/null +++ b/src/DUNE/Parsers/HDF5Reader.cpp @@ -0,0 +1,215 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Miguel Aguiar * +//*************************************************************************** + +#include +#include + +#include "DUNE/I18N.hpp" + +// This component depends on the h5cpp library +// (https://github.com/ess-dmsc/h5cpp.com). +// To enable support for h5cpp, set the H5CPP flag to 1 when calling CMake. +// If support for h5cpp is not enabled, the compilation will not fail, but +// the constructor of HDF5Reader will throw. +#if DUNE_H5CPP_ENABLED +# include "h5cpp/hdf5.hpp" +#else +// Define hdf5::file::File to avoid compilation errors due to incomplete type +namespace hdf5 +{ + namespace file + { + class File + {}; + } // namespace file +} // namespace hdf5 +#endif + +#include "HDF5Reader.hpp" + +namespace DUNE +{ + namespace Parsers + { + HDF5Reader::HDF5Reader(std::string const& filename) + : m_file(std::make_unique()) + { +#ifndef DUNE_H5CPP_ENABLED + throw std::runtime_error( + DTR("HDF5Reader::HDF5Reader(): support for HDF5 i/o is not " + "enabled.")); +#else + try + { + *m_file = + hdf5::file::open(filename, hdf5::file::AccessFlags::READONLY); + + if (m_file == nullptr) + throw; + } + catch (...) + { + throw std::runtime_error( + DTR("HDF5Reader::HDF5Reader(): unable to open file.")); + } +#endif + } + + HDF5Reader::~HDF5Reader() = default; + +#ifdef DUNE_H5CPP_ENABLED + inline bool + dsetExists(File const* f, std::string const& path) + { + return f->root().has_dataset(path); + return false; + } +#endif + + bool + HDF5Reader::datasetExists(std::string const& path) const + { +#ifdef DUNE_H5CPP_ENABLED + return dsetExists(m_file.get(), path); +#else + return false; +#endif + } + + template + HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const& path) const + { +#ifdef DUNE_H5CPP_ENABLED + if (!dsetExists(m_file.get(), path)) + throw std::runtime_error( + DTR("HDF5Reader::getDataset(): The requested dataset does not " + "exist.")); + + auto dset = m_file->root().get_dataset(path); + auto dimensions = + hdf5::dataspace::Simple(dset.dataspace()).current_dimensions(); + + // Total number of gridpoints is the product of the sizes of all + // dimensions. + size_t size = std::accumulate(std::begin(dimensions), + std::end(dimensions), + 1, + std::multiplies<>()); + + std::vector data(size); + dset.read(data); + + // Convert elements of dimensions to size_t + std::vector dimensions_(dimensions.size()); + + std::copy(std::begin(dimensions), + std::end(dimensions), + std::begin(dimensions_)); + + return {dimensions_, data}; +#else + return {}; +#endif + } + + template + std::vector + HDF5Reader::getAttribute(std::string const& path, + std::string const& attribute) const + { +#ifdef DUNE_H5CPP_ENABLED + auto node = hdf5::node::get_node(m_file->root(), path); + auto attributes = node.attributes; + + if (!attributes.exists(attribute)) + throw std::runtime_error( + DTR("HDF5Reader::getAttribute(): requested attribute does not " + "exist.")); + + auto attr_handle = attributes[attribute]; + + std::vector data(attr_handle.dataspace().size()); + + attr_handle.read(data); + + return data; +#else + return {}; +#endif + } + + // Template specialization declarations for getDataset + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + // Template specialization declarations for getAttribute + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + } // namespace StreamSpeed +} // namespace Simulators diff --git a/src/Simulators/StreamSpeed/HDF5Reader.hpp b/src/DUNE/Parsers/HDF5Reader.hpp similarity index 59% rename from src/Simulators/StreamSpeed/HDF5Reader.hpp rename to src/DUNE/Parsers/HDF5Reader.hpp index 17cdd9fc72..45fcaefd10 100644 --- a/src/Simulators/StreamSpeed/HDF5Reader.hpp +++ b/src/DUNE/Parsers/HDF5Reader.hpp @@ -43,63 +43,60 @@ namespace hdf5 } } // namespace hdf5 -namespace Simulators +namespace DUNE { - namespace StreamSpeed + namespace Parsers { - namespace StreamGenerator - { - using hdf5::file::File; + using hdf5::file::File; - //! Simplifies reading data and attributes from HDF5 format files. - class HDF5Reader - { - public: - //! Constructor. - //! @param[in] filename path to an hdf5 file. - HDF5Reader(std::string const& filename); + //! Simplifies reading data and attributes from HDF5 format files. + class HDF5Reader + { + public: + //! Constructor. + //! @param[in] filename path to an hdf5 file. + HDF5Reader(std::string const& filename); - //! Destructor. - ~HDF5Reader(); + //! Destructor. + ~HDF5Reader(); - //! Structure holding an arbitrary multidimensional HDF5 dataset. - template - struct HDF5Dataset - { - //! Number of points in each dimension. - std::vector dimensions; - //! The data values in row-major (C-style) order. - std::vector data; - }; + //! Structure holding an arbitrary multidimensional HDF5 dataset. + template + struct HDF5Dataset + { + //! Number of points in each dimension. + std::vector dimensions; + //! The data values in row-major (C-style) order. + std::vector data; + }; - //! Check if a dataset exists in the given file. - //! @param[in] path path to the dataset in the file. - //! @return whether the dataset exists in the file. - bool - datasetExists(std::string const& path) const; + //! Check if a dataset exists in the given file. + //! @param[in] path path to the dataset in the file. + //! @return whether the dataset exists in the file. + bool + datasetExists(std::string const& path) const; - //! Get a dataset and its dimensions. - //! @param[in] path path to the dataset in the file. - //! @return structure containing the data and the dataset dimensions. - template - HDF5Dataset - getDataset(std::string const& path) const; + //! Get a dataset and its dimensions. + //! @param[in] path path to the dataset in the file. + //! @return structure containing the data and the dataset dimensions. + template + HDF5Dataset + getDataset(std::string const& path) const; - //! Get an attribute. - //! @param[in] path path to the node in the file where the attribute is - //! stored. - //! @param[in] attribute name of the attribute to get. - //! @return the attribute's data. - template - std::vector - getAttribute(std::string const& path, - std::string const& attribute) const; + //! Get an attribute. + //! @param[in] path path to the node in the file where the attribute is + //! stored. + //! @param[in] attribute name of the attribute to get. + //! @return the attribute's data. + template + std::vector + getAttribute(std::string const& path, + std::string const& attribute) const; - private: - //! Handle to an HDF5 file. - std::unique_ptr m_file; - }; - } // namespace StreamGenerator + private: + //! Handle to an HDF5 file. + std::unique_ptr m_file; + }; } // namespace StreamSpeed } // namespace Simulators diff --git a/src/Simulators/CTD/CTDGenerator.cpp b/src/Simulators/CTD/CTDGenerator.cpp new file mode 100644 index 0000000000..44de311f7d --- /dev/null +++ b/src/Simulators/CTD/CTDGenerator.cpp @@ -0,0 +1,69 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +// DUNE headers. +#include + +#include "CTDGenerator.hpp" + +namespace Simulators +{ + namespace CTD + { + CTDGenerator::CTDGenerator(float default_temp, float default_sal): + m_default_temp(default_temp), + m_default_sal(default_sal) + {} + + float + CTDGenerator::getTemperature(std::array pos) + { + return m_default_temp; + } + + float + CTDGenerator::getSalinity(std::array pos) + { + return m_default_sal; + } + + void + CTDGenerator::setDefaultTemperature(float temp) + { + m_default_temp = temp; + } + + void + CTDGenerator::setDefaultSalinity(float sal) + { + m_default_sal = sal; + } + + } +} \ No newline at end of file diff --git a/src/Simulators/CTD/CTDGenerator.hpp b/src/Simulators/CTD/CTDGenerator.hpp new file mode 100644 index 0000000000..3f8b49498c --- /dev/null +++ b/src/Simulators/CTD/CTDGenerator.hpp @@ -0,0 +1,84 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +#ifndef SIMULATORS_CTD_GENERATOR_HPP_INCLUDED_ +#define SIMULATORS_CTD_GENERATOR_HPP_INCLUDED_ + +// DUNE headers. +#include + +namespace Simulators +{ + namespace CTD + { + class CTDGenerator + { + public: + //! Constructor. + //! @param[in] default_temp temperature default value. + //! @param[in] default_sal salinity default value. + CTDGenerator(float default_temp, float default_sal); + + //! Destructor. + ~CTDGenerator() = default; + + //! Get temperature value from model. + //! @param[in] pos 4-dimensional array with vehicle + //! position and time. + //! @return temperature value from model. + virtual float + getTemperature(std::array pos); + + //! Get salinity value from model. + //! @param[in] pos 4-dimensional array with vehicle + //! position and time. + //! @return temperature value from model. + virtual float + getSalinity(std::array pos); + + //! Set default temperature value. + //! @param[in] temp new default temperature value. + void + setDefaultTemperature(float temp); + + //! Set default salinity value. + //! @param[in] sal new default salinity value. + void + setDefaultSalinity(float sal); + + protected: + //! Default temperature value. + float m_default_temp; + //! Default salinity value. + float m_default_sal; + }; + } +} + +#endif \ No newline at end of file diff --git a/src/Simulators/CTD/CTDGeneratorFactory.hpp b/src/Simulators/CTD/CTDGeneratorFactory.hpp new file mode 100644 index 0000000000..b25329257d --- /dev/null +++ b/src/Simulators/CTD/CTDGeneratorFactory.hpp @@ -0,0 +1,72 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +#ifndef SIMULATORS_CTD_CTD_GENERATOR_FACTORY_HPP_INCLUDED_ +#define SIMULATORS_CTD_CTD_GENERATOR_FACTORY_HPP_INCLUDED_ + +#include + +#include "DUNE/I18N.hpp" + +#include "MOHIDCTDGenerator.hpp" +#include "CTDGenerator.hpp" + +namespace Simulators +{ + namespace CTD + { + //! Factory method for the CTD model generator. + //! @param[in] config structure with configuration fields. + //! @return handle to CTD model generator. + template + std::unique_ptr + factory(Configuration const& config) + { + if (config.type == "Constant") + return std::make_unique( + config.default_temp, config.default_sal); + + if (config.type == "MOHID Model Data") + { + MOHIDArguments mohid_args; + mohid_args.file_path = config.file_path; + + mohid_args.surface_radius = config.surface_radius; + mohid_args.depth_radius = config.depth_radius; + + return std::make_unique( + mohid_args, config.default_temp, config.default_sal); + } + else + throw std::runtime_error(DTR("Unknown stream velocity source type.")); + } + } +} + +#endif \ No newline at end of file diff --git a/src/Simulators/CTD/MOHIDCTDGenerator.cpp b/src/Simulators/CTD/MOHIDCTDGenerator.cpp new file mode 100644 index 0000000000..092927e460 --- /dev/null +++ b/src/Simulators/CTD/MOHIDCTDGenerator.cpp @@ -0,0 +1,165 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +// DUNE headers. +#include + +#include +#include "MOHIDCTDGenerator.hpp" + +using DUNE_NAMESPACES; +using namespace Oc; + +namespace Simulators +{ + namespace CTD + { + MOHIDCTDGenerator::MOHIDCTDGenerator(MOHIDArguments args, + double default_temp, double default_sal): + m_args(args), + CTDGenerator(default_temp, default_sal), + m_tree_generator(args.file_path) + { + m_model_time.first = 0; + m_model_time.second = 1; + + m_temp_model.first = std::move( + m_tree_generator.getTree( + "temperature", m_model_time.first)); + m_temp_model.second = std::move( + m_tree_generator.getTree( + "temperature", m_model_time.second)); + m_sal_model.first = std::move( + m_tree_generator.getTree( + "salinity", m_model_time.first)); + m_sal_model.second = std::move( + m_tree_generator.getTree( + "salinity", m_model_time.second)); + } + + float + MOHIDCTDGenerator::timeInterpolation(QuantityModel* model, + std::array pos) + { + float lower_val = spaceInterpolation(model->first.get(), + {pos[0], pos[1], pos[2]}); + float upper_val = spaceInterpolation(model->second.get(), + {pos[0], pos[1], pos[2]}); + + //Invalid + if (lower_val <= c_invalid || upper_val <= c_invalid) + return c_invalid; + + //Calculate linear parameters (value = m * time + b) + double m = (upper_val - lower_val)/ + (m_model_time.second - m_model_time.first); + double b = lower_val - m * m_model_time.first; + + return m*pos[3]+b; + } + + float + MOHIDCTDGenerator::spaceInterpolation(OcTree* otree, + std::array pos) + { + Point p(pos[0], pos[1], pos[2]); + Bounds search_area(p, m_args.surface_radius, + m_args.surface_radius, m_args.depth_radius); + + std::vector items; + otree->search(search_area, items); + + if (items.size() == 0) + return c_invalid; + + //Temperature calculated using: + //Inverse distance weighting + double denominator = 0; + double numerator = 0; + for (auto item : items) + { + double distance = p.distance(Point(item.x, item.y, item.z)); + if (distance == 0) + return item.value; + + double inverse_d = 1/distance; + denominator += inverse_d; + numerator += inverse_d*item.value; + } + + return numerator/denominator; + } + + void + MOHIDCTDGenerator::updateModel() + { + m_model_time.first = m_model_time.second; + m_model_time.second += 1; + + m_temp_model.first.swap(m_temp_model.second); + m_temp_model.second = std::move( + m_tree_generator.getTree( + "temperature", m_model_time.second)); + + m_sal_model.first.swap(m_sal_model.second); + m_sal_model.second = std::move( + m_tree_generator.getTree( + "salinity", m_model_time.second)); + } + + float + MOHIDCTDGenerator::getTemperature(std::array pos) + { + if (m_model_time.second <= pos[3]) + updateModel(); + + float temp = timeInterpolation(&m_temp_model, pos); + + if (temp <= c_invalid) + return m_default_temp; + + return temp; + } + + float + MOHIDCTDGenerator::getSalinity(std::array pos) + { + if (m_model_time.second <= pos[3]) + updateModel(); + + float sal = timeInterpolation(&m_sal_model, pos); + + if (sal <= c_invalid) + return m_default_sal; + + return sal; + } + } +} + diff --git a/src/Simulators/CTD/MOHIDCTDGenerator.hpp b/src/Simulators/CTD/MOHIDCTDGenerator.hpp new file mode 100644 index 0000000000..1d67fbc8dd --- /dev/null +++ b/src/Simulators/CTD/MOHIDCTDGenerator.hpp @@ -0,0 +1,117 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +#ifndef SIMULATORS_MOHID_CTD_GENERATOR_HPP_INCLUDED_ +#define SIMULATORS_MOHID_CTD_GENERATOR_HPP_INCLUDED_ + +// DUNE headers. +#include + +#include +#include "MOHIDTreeGenerator.hpp" +#include "CTDGenerator.hpp" + +using DUNE_NAMESPACES; +using namespace Oc; + +namespace Simulators +{ + namespace CTD + { + //! Arguments for MOHID model generator + struct MOHIDArguments + { + //! Path to data file. + std::string file_path; + //! Interpolation radius for latitude and longitude + //! in degrees. + double surface_radius; + //! Interpolation radius for depth in meters. + double depth_radius; + }; + + class MOHIDCTDGenerator : public CTDGenerator + { + public: + //! Constructor. + //! @param[in] args Arguments for MOHID generator. + //! @param[in] default_temp Default temperature value. + //! @param[in] default_sal Default salinity value. + MOHIDCTDGenerator(MOHIDArguments args, + double default_temp, double default_sal); + + //! Destructor. + ~MOHIDCTDGenerator() = default; + + virtual float + getTemperature(std::array pos); + + virtual float + getSalinity(std::array pos); + + + private: + //! Arguments for MOHID model generator. + MOHIDArguments m_args; + //! Tree generator for MOHID model data. + MOHIDTreeGenerator m_tree_generator; + //! Current loaded data times, in decimal hours. + std::pair m_model_time; + + typedef std::pair, + std::unique_ptr> QuantityModel; + //! Current loaded temperature model. + QuantityModel m_temp_model; + //! Current loaded salinity model. + QuantityModel m_sal_model; + + //! Interpolate values of temperature in time, between 2 + //! loaded datasets. + //! @param[in] model Model to interpolate. + //! @param[in] pos 4-dimensional array of vehicle position + //! and time. + //! @return time interpolated value. + float + timeInterpolation(QuantityModel* model, std::array pos); + + //! Interpolate values of temperature in space. + //! @param[in] otree Tree to interpolate. + //! @param[in] pos 3-dimensional array of vehicle position. + //! @return position interpolated value. + float + spaceInterpolation(OcTree* otree, std::array pos); + + //! Load dataset for next time frame. + void + updateModel(); + }; + } +} + +#endif \ No newline at end of file diff --git a/src/Simulators/CTD/MOHIDTreeGenerator.cpp b/src/Simulators/CTD/MOHIDTreeGenerator.cpp new file mode 100644 index 0000000000..f4cf724c60 --- /dev/null +++ b/src/Simulators/CTD/MOHIDTreeGenerator.cpp @@ -0,0 +1,281 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +// DUNE headers. +#include +#include "DUNE/I18N.hpp" +#include "DUNE/Parsers/HDF5Reader.hpp" + +#include +#include +#include "MOHIDTreeGenerator.hpp" + +#include + +using DUNE_NAMESPACES; +using namespace Oc; + +namespace Simulators +{ + namespace CTD + { + // Auxiliary + std::string + time2string(unsigned time) + { + std::stringstream sin; sin.clear(); + sin << time + 1; + std::string time_str = std::string(5-sin.str().size(), '0'); + time_str += sin.str(); + + return time_str; + } + + MOHIDTreeGenerator::MOHIDTreeGenerator(std::string file_path): + m_file(file_path) + { + get2DCoordinates(); + } + + std::unique_ptr + MOHIDTreeGenerator::getTree(std::string quantity, unsigned time) + { + //Get path for time slice + std::string time_str = time2string(time); + std::string data_path = "Results/" + quantity + "/" + quantity + "_" + time_str; + + //Get data + auto data = m_file.getDataset(data_path); + + std::vector cell_depth; + if (is3D()) + { + std::string height_path = "Grid/VerticalZ/Vertical_" + time_str; + cell_depth = getZCoordinates(height_path); + + // Check dimensions + if (data.data.size() != cell_depth.size()) + throw std::runtime_error(DTR("MOHIDTreeGenerator::fillTree(): " + "data-vertical grid dimension mismatch.")); + + return fillTree(&data.data, &cell_depth); + } + else + { + return fillTree(&data.data); + } + } + + std::unique_ptr + MOHIDTreeGenerator::fillTree(std::vector* quantity, + std::vector* cell_depth) + { + if (!quantity || !cell_depth) + throw std::runtime_error(DTR("MOHIDTreeGenerator::fillTree(): " + "Null pointer.")); + + if (quantity->size() == 0 || cell_depth->size() == 0) + throw std::runtime_error(DTR("MOHIDTreeGenerator::fillTree(): " + "Empty data vector.")); + + //Auxiliary objects to fill otree + std::vector data; + OcTree::Item item; + Bounds* bounds = 0; + + //Set data points + for(unsigned offset = 0; offset < quantity->size(); ++offset) + { + if (quantity->at(offset) > c_invalid && + cell_depth->at(offset) > c_invalid) + { + item.x = m_cell_lat[offset % m_cell_lat.size()]; + item.y = m_cell_lon[offset % m_cell_lon.size()]; + item.z = cell_depth->at(offset); + item.value = quantity->at(offset); + data.push_back(item); + + //Expand bounds + if (!bounds) + bounds = new Bounds(Point(item.x, item.y, item.z)); + else + bounds->cover(Point(item.x, item.y, item.z)); + } + } + + std::unique_ptr otree = std::make_unique(*bounds); + delete bounds; + for (unsigned i = 0; i < data.size(); ++i) + otree->insert(data[i]); + + return otree; + } + + std::unique_ptr + MOHIDTreeGenerator::fillTree(std::vector* quantity) + { + if (!quantity) + throw std::runtime_error(DTR("MOHIDTreeGenerator::fillTree(): " + "Null pointer.")); + + if (quantity->size() == 0) + throw std::runtime_error(DTR("MOHIDTreeGenerator::fillTree(): " + "Empty data vector.")); + + //Auxiliary objects to fill otree + std::vector data; + OcTree::Item item; + Bounds* bounds = 0; + + //Set data points + for(unsigned offset = 0; offset < quantity->size(); ++offset) + { + if (quantity->at(offset) > c_invalid) + { + item.x = m_cell_lat[offset % m_cell_lat.size()]; + item.y = m_cell_lon[offset % m_cell_lon.size()]; + item.z = 0; + item.value = quantity->at(offset); + data.push_back(item); + + //Expand bounds + if (!bounds) + bounds = new Bounds(Point(item.x, item.y, item.z)); + else + bounds->cover(Point(item.x, item.y, item.z)); + } + } + + std::unique_ptr otree = std::make_unique(*bounds); + delete bounds; + for (unsigned i = 0; i < data.size(); ++i) + otree->insert(data[i]); + + return otree; + } + + std::vector + MOHIDTreeGenerator::grid2CellSurface(std::vector* data, + std::vector* dim) + { + size_t dimentions_total = std::accumulate(dim->begin(), dim->end(), + 1.0, + std::multiplies()); + if (data->size() != dimentions_total) + throw std::runtime_error(DTR("MOHIDTreeGenerator::grid2CellSurface(): " + "dimention mismatch.")); + + std::vector cells; + // Grid x boundary + unsigned boundary = dim->at(1) * (dim->at(0)-1) - 1; + for (unsigned offset = 0; offset < boundary; ++offset) + { + //Grid y boundary + if ((offset+1) % dim->at(1) == 0) + continue; + + //Average grid vertices + auto val = data->at(offset); + val += data->at(offset+1); + val += data->at(offset + dim->at(1)); + val += data->at(offset + dim->at(1)+1); + + cells.push_back(val/4); + } + + return cells; + } + + std::vector + MOHIDTreeGenerator::grid2CellVertical(std::vector* data, + std::vector* dim) + { + // Check size + size_t dimentions_total = std::accumulate(dim->begin(), dim->end(), + 1.0, + std::multiplies()); + if (data->size() != dimentions_total) + throw std::runtime_error(DTR("MOHIDTreeGenerator::grid2CellVertical(): " + "dimention mismatch.")); + + std::vector cells; + unsigned size_2d = dim->at(2) * dim->at(1); + // Grid z boundary + unsigned boundary = size_2d * (dim->at(0) - 1); + for (unsigned offset = 0; offset < boundary; ++offset) + { + //Average grid vertices + auto val = data->at(offset); + val += data->at(offset + size_2d); + + cells.push_back(val/2); + } + + return cells; + } + + void + MOHIDTreeGenerator::get2DCoordinates() + { + auto lat = m_file.getDataset("Grid/Latitude"); + auto lon = m_file.getDataset("Grid/Longitude"); + + if (lat.dimensions != lon.dimensions) + throw std::runtime_error(DTR("MOHIDTreeGenerator::MOHIDTreeGenerator():" + "lat-lon dimension mismatch.")); + + m_cell_lat = grid2CellSurface(&lat.data, &lat.dimensions); + m_cell_lon = grid2CellSurface(&lon.data, &lon.dimensions); + } + + std::vector + MOHIDTreeGenerator::getZCoordinates(std::string path) + { + // Get depth + auto depth = m_file.getDataset(path); + + // Transform vertical grid values to cell values + std::vector cell_depth = grid2CellVertical(&depth.data, &depth.dimensions); + + // Check dimensions + if (cell_depth.size() % m_cell_lat.size() != 0 || + cell_depth.size() % m_cell_lon.size() != 0) + throw std::runtime_error(DTR("MOHIDTreeGenerator::MOHIDTreeGenerator():" + "lat/lon - depth dimension mismatch.")); + + return cell_depth; + } + + bool + MOHIDTreeGenerator::is3D() + { + return m_file.datasetExists("Grid/VerticalZ/Vertical_00001"); + } + } +} diff --git a/src/Simulators/CTD/MOHIDTreeGenerator.hpp b/src/Simulators/CTD/MOHIDTreeGenerator.hpp new file mode 100644 index 0000000000..42287e14e6 --- /dev/null +++ b/src/Simulators/CTD/MOHIDTreeGenerator.hpp @@ -0,0 +1,131 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Luis Venancio * +//*************************************************************************** + +#ifndef SIMULATORS_MOHID_TREE_GENERATOR_HPP_INCLUDED_ +#define SIMULATORS_MOHID_TREE_GENERATOR_HPP_INCLUDED_ + +// DUNE headers. +#include + +#include +#include +#include "DUNE/Parsers/HDF5Reader.hpp" + +using DUNE_NAMESPACES; + +// OcTree specific namespace +using namespace Oc; + +namespace Simulators +{ + namespace CTD + { + //! Constante value for invalid comparison. + static const float c_invalid = pow(-10,15); + + class MOHIDTreeGenerator + { + public: + //! Constructor. + //! @param[in] file_path Path to MOHID data file. + MOHIDTreeGenerator(std::string file_path); + + //! Destructor. + ~MOHIDTreeGenerator() = default; + + //! Get tree for specified time. + //! @param[in] quatity Name of quantity to load from + //! MOHID data file. + //! @return unique pointer to data tree. + std::unique_ptr + getTree(std::string quantity, unsigned time); + + //! Returns 3D data flag. + //! @return true if data is in 3D space, false if 2D space. + bool + is3D(); + + private: + //! Handler of HDF5 interface. + HDF5Reader m_file; + //! Latitude of cells in row-major order. + std::vector m_cell_lat; + //! Longitude of cells in row-major order. + std::vector m_cell_lon; + + //! Fill data tree from file data (3D). + //! @param[in] data Pointer to cell data, + //! in row-major order. + //! @param[in] vert Pointer to cell depth + //! values, in row-major order. + //! @return Unique poitner to data tree. + std::unique_ptr + fillTree(std::vector* quantity, + std::vector* cell_depth); + + //! Fill data tree from file data (2D). + //! @param[in] data Pointer to cell data, + //! in row-major order. + //! @return Unique pointer to data tree. + std::unique_ptr + fillTree(std::vector* quantity); + + //! Average latitude and longitude grid + //! values to cell values. + //! @param[in] data Latitude or Longitude grid to, + //! convert in row-major order. + //! @param[in] dim Data dimensions. + //! @return Converted cell values in row-major order. + std::vector + grid2CellSurface(std::vector* data, + std::vector* dim); + + //! Average depth grid values to cell values. + //! @param[in] data Depth grid to convert, + //! in row-major order. + //! @param[in] dim Data dimensions. + //! @return Converted cell values in row-major order. + std::vector + grid2CellVertical(std::vector* data, + std::vector* dim); + + //! Fill lat/lon cell values. + void + get2DCoordinates(); + + //! Get Z coordinate vector. + //! @param[in] path Path to depth data in hdf5 file. + //! @return Vector with cell center values. + std::vector + getZCoordinates(std::string path); + }; + } +} + +#endif \ No newline at end of file diff --git a/src/Simulators/CTD/Task.cpp b/src/Simulators/CTD/Task.cpp index 805624ce50..ca135de3a9 100644 --- a/src/Simulators/CTD/Task.cpp +++ b/src/Simulators/CTD/Task.cpp @@ -25,33 +25,58 @@ // http://ec.europa.eu/idabc/eupl.html. * //*************************************************************************** // Author: Pedro Calado * +// Author: Luis Venancio * //*************************************************************************** +// ISO C++ 98 headers. +#include + // DUNE headers. #include +#include "CTDGenerator.hpp" +#include "CTDGeneratorFactory.hpp" using DUNE_NAMESPACES; namespace Simulators { //! CTD (Conductivity, Temperature, Depth) sensor simulator. - //! All data is generated using a mean and standard deviation. - //! + //! Data is pulled from source data files. + //! + //! Condutivity is obtained by appling a standard UNESCO1983 algorithm + //! using salinity and tempearture data. + //! //! The depth value is generated by listening to DUNE::IMC::SimulatedState //! and applying a standard deviation to received values. + //! @author Pedro Calado + //! @author Luis Venancio + namespace CTD { - //! %Task arguments. + + //! Seconds in hour. + constexpr double c_seconds_to_hour= 3600; + + //! Task arguments. struct Arguments { + //! Type of model to use. + std::string type; + //! Path to data file. + std::string file_path; + //! Default temperature. + double default_temp; + //! Default salinity. + double default_sal; + //! Interpolation radius of latitude and longitude, + //! in degrees. + double surface_radius; + //! Interpolation radius of depth, in meters. + double depth_radius; //! Standard deviation of temperature measurements. double std_dev_temp; - //! Mean temperature value. - float mean_temp; - //! Standard deviation of conductivity measurements. - double std_dev_cond; - //! Mean conductivity value. - float mean_cond; + //! Standard deviation of salinity measurements. + double std_dev_sal; //! Standard deviation of depth measurements. double std_dev_depth; //! Name of Pseudo-Random Number Generator to use. @@ -60,7 +85,7 @@ namespace Simulators int prng_seed; }; - //! %SVS simulator task. + //! CTD simulator task. struct Task: public Tasks::Periodic { //! Temperature. @@ -71,7 +96,9 @@ namespace Simulators IMC::Conductivity m_cond; //! Current salinity. IMC::Salinity m_salinity; + //! Current depth. IMC::Depth m_depth; + //! Current pressure. IMC::Pressure m_pressure; //! Last received simulated state. IMC::SimulatedState m_sstate; @@ -79,27 +106,49 @@ namespace Simulators Random::Generator* m_prng; //! Task arguments. Arguments m_args; + //! Delta for time keeping + Delta current_time; + //! Pointer to data generator + std::unique_ptr m_ctd_generator; + //! Constructor. + //! @param[in] name task name. + //! @param[in] ctx context. Task(const std::string& name, Tasks::Context& ctx): Tasks::Periodic(name, ctx), m_prng(NULL) { - // Retrieve configuration values. param("Standard Deviation - Temperature", m_args.std_dev_temp) - .defaultValue("1.0"); - - param("Mean Value - Temperature", m_args.mean_temp) - .defaultValue("14.0"); - - param("Standard Deviation - Conductivity", m_args.std_dev_cond) - .defaultValue("1.0"); + .defaultValue("0.5"); - param("Mean Value - Conductivity", m_args.mean_cond) - .defaultValue("4.0"); + param("Standard Deviation - Salinity", m_args.std_dev_sal) + .defaultValue("0.5"); param("Standard Deviation - Depth", m_args.std_dev_depth) .defaultValue("0.1"); + param("Model Type", m_args.type) + .defaultValue("Constant"); + + param("Data File Path", m_args.file_path) + .description("Full path to data file"); + + param("Default - Temperature", m_args.default_temp) + .description("Default temperature value") + .defaultValue("5.0"); + + param("Default - Salinity", m_args.default_sal) + .description("Default salinity value") + .defaultValue("10.0"); + + param("Interpolation Radius - Surface", m_args.surface_radius) + .description("Radius to search for points in the lat/lon dimensions") + .defaultValue("0.01"); + + param("Interpolation Radius - Depth", m_args.depth_radius) + .description("Radius to search for points in the depth dimension") + .defaultValue("15"); + param("PRNG Type", m_args.prng_type) .description("Pseudo-Random Number Generator type. Examples: \"c_fsr256\", \"krng\"") .defaultValue(Random::Factory::c_default); @@ -121,11 +170,26 @@ namespace Simulators requestDeactivation(); } - //! Acquire resources. Initializes the random number generator + //! Acquire resources. void onResourceAcquisition(void) { - //! Initialize the random number generator. + try + { + m_ctd_generator = factory(m_args); + } + catch (std::exception const& e) + { + err(DTR("Unable to obtain model data:" + "\n" + "\t%s\n" + "\tFalling back to default values."), + e.what()); + + m_args.type = "Constant"; + m_ctd_generator = factory(m_args); + } + m_prng = Random::Factory::create(m_args.prng_type, m_args.prng_seed); } @@ -145,18 +209,93 @@ namespace Simulators { setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); requestActivation(); + current_time.reset(); } m_sstate = *msg; } + //! Get temperature value from model. + //! @return temperature value. + float + getTemperature() + { + double lat = m_sstate.lat; + double lon = m_sstate.lon; + double height = m_sstate.height; + + Coordinates::WGS84::displace(m_sstate.x, m_sstate.y, m_sstate.z, + &lat, &lon, &height); + + std::array pos = {Angles::degrees(lat), Angles::degrees(lon), + m_sstate.height, current_time.check()/c_seconds_to_hour}; + + float temp; + + try + { + temp = m_ctd_generator->getTemperature(pos); + } + catch(const std::exception& e) + { + err(DTR("Unable to obtain model data:" + "\n" + "\t%s\n" + "\tFalling back to default values."), + e.what()); + + m_args.type = "Constant"; + m_ctd_generator = factory(m_args); + temp = m_ctd_generator->getTemperature(pos); + } + + return temp; + } + + //! Get salinity value from model. + //! @return salinity value. + float + getSalinity() + { + double lat = m_sstate.lat; + double lon = m_sstate.lon; + double height = m_sstate.height; + + Coordinates::WGS84::displace(m_sstate.x, m_sstate.y, m_sstate.z, + &lat, &lon, &height); + + std::array pos = {Angles::degrees(lat), Angles::degrees(lon), + m_sstate.height, + current_time.check()/c_seconds_to_hour}; + float sal; + + try + { + sal = m_ctd_generator->getSalinity(pos); + } + catch(const std::exception& e) + { + err(DTR("Unable to obtain model data:" + "\n" + "\t%s\n" + "\tFalling back to default values."), + e.what()); + + m_args.type = "Constant"; + m_ctd_generator = factory(m_args); + sal = m_ctd_generator->getSalinity(pos); + } + + return sal; + } + //! If active, computes all values using random value generators and dispatches: //! * @publish DUNE::IMC::Temperature //! * @publish DUNE::IMC::Salinity //! * @publish DUNE::IMC::Depth + //! * @publish DUNE::IMC::Pressure //! * @publish DUNE::IMC::Conductivity //! * @publish DUNE::IMC::SoundSpeed - //! * @publish DUNE::IMC::Pressure void task(void) { @@ -165,10 +304,12 @@ namespace Simulators return; m_temp.setTimeStamp(); - m_temp.value = m_args.mean_temp + m_prng->gaussian() * m_args.std_dev_temp; + m_temp.value = getTemperature(); + m_temp.value = m_temp.value + m_prng->gaussian() * m_args.std_dev_temp; - m_cond.setTimeStamp(m_temp.getTimeStamp()); - m_cond.value = m_args.mean_cond + m_prng->gaussian() * m_args.std_dev_cond; + m_salinity.setTimeStamp(m_temp.getTimeStamp()); + m_salinity.value = getSalinity(); + m_salinity.value = m_salinity.value + m_prng->gaussian() * m_args.std_dev_sal; m_depth.setTimeStamp(m_temp.getTimeStamp()); m_depth.value = std::max(m_sstate.z + m_prng->gaussian() * m_args.std_dev_depth, 0.0); @@ -177,8 +318,8 @@ namespace Simulators m_pressure.setTimeStamp(m_temp.getTimeStamp()); m_pressure.value = (m_depth.value * c_gravity * c_seawater_density + c_sea_level_pressure) / c_pascal_per_bar; - m_salinity.setTimeStamp(m_temp.getTimeStamp()); - m_salinity.value = UNESCO1983::computeSalinity(m_cond.value, m_pressure.value, m_temp.value); + m_cond.setTimeStamp(m_temp.getTimeStamp()); + m_cond.value = UNESCO1983::computeConductivity(m_salinity.value, m_pressure.value, m_temp.value); m_sspeed.setTimeStamp(m_temp.getTimeStamp()); m_sspeed.value = (m_salinity.value < 0.0) ? -1.0 : UNESCO1983::computeSoundSpeed(m_salinity.value, m_pressure.value, m_temp.value); diff --git a/src/Simulators/Environment/Bounds.hpp b/src/Simulators/Environment/Bounds.hpp deleted file mode 100644 index 0f17336e92..0000000000 --- a/src/Simulators/Environment/Bounds.hpp +++ /dev/null @@ -1,163 +0,0 @@ -//*************************************************************************** -// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * -// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * -//*************************************************************************** -// This file is part of DUNE: Unified Navigation Environment. * -// * -// Commercial Licence Usage * -// Licencees holding valid commercial DUNE licences may use this file in * -// accordance with the commercial licence agreement provided with the * -// Software or, alternatively, in accordance with the terms contained in a * -// written agreement between you and Faculdade de Engenharia da * -// Universidade do Porto. For licensing terms, conditions, and further * -// information contact lsts@fe.up.pt. * -// * -// Modified European Union Public Licence - EUPL v.1.1 Usage * -// Alternatively, this file may be used under the terms of the Modified * -// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * -// included in the packaging of this file. You may not use this work * -// except in compliance with the Licence. Unless required by applicable * -// law or agreed to in writing, software distributed under the Licence is * -// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * -// ANY KIND, either express or implied. See the Licence for the specific * -// language governing permissions and limitations at * -// https://github.com/LSTS/dune/blob/master/LICENCE.md and * -// http://ec.europa.eu/idabc/eupl.html. * -//*************************************************************************** -// Author: Eduardo Marques * -//*************************************************************************** - -#ifndef SIMULATORS_ENVIRONMENT_BOUNDS_HPP_INCLUDED_ -#define SIMULATORS_ENVIRONMENT_BOUNDS_HPP_INCLUDED_ - -// ISO C++ headers. -#include -#include -#include - -// DUNE headers. -#include - -// Local headers. -#include "Point.hpp" - -namespace Simulators -{ - namespace Environment - { - struct Bounds - { - double min_x, max_x, min_y, max_y; - - Bounds(const Point& p, double r): - min_x(p.x - r), max_x(p.x + r), min_y(p.y - r), max_y(p.y + r) - { } - - Bounds(const Point& p): - min_x(p.x), max_x(p.x), min_y(p.y), max_y(p.y) - { } - - Bounds(const Bounds& parent, const Point& lim, int dir) - { - switch (dir) - { - case DIR_NW: - min_x = lim.x; - max_x = parent.max_x; - min_y = parent.min_y; - max_y = lim.y; - break; - case DIR_NE: - min_x = lim.x; - max_x = parent.max_x; - min_y = lim.y; - max_y = parent.max_y; - break; - case DIR_SW: - min_x = parent.min_x; - max_x = lim.x; - min_y = parent.min_y; - max_y = lim.y; - break; - case DIR_SE: - default: - min_x = parent.min_x; - max_x = lim.x; - min_y = lim.y; - max_y = parent.max_y; - break; - } - } - - Bounds - quadrant(int dir) const - { - return Bounds(*this, midpoint(), dir); - } - - template - std::pair - quadrant(const T& p) const - { - Point lim = midpoint(); - int q = lim.direction(p); - return std::pair(q, Bounds(*this, lim, q)); - } - - bool - intersects(const Bounds& other) const - { - if (min_x > other.max_x || other.min_x > max_x) - return false; - if (min_y > other.max_y || other.min_y > max_y) - return false; - return true; - } - - template - void - cover(const T& p) - { - if (p.x < min_x) - min_x = p.x; - else if (p.x > max_x) - max_x = p.x; - - if (p.y < min_y) - min_y = p.y; - else if (p.y > max_y) - max_y = p.y; - } - - template - bool - contains(const T& p) const - { - return p.x >= min_x && p.x <= max_x && p.y >= min_y && p.y <= max_y; - } - - Point - midpoint() const - { - return Point(0.5 * (min_x + max_x), 0.5 * (min_y + max_y)); - } - - double - width() const - { - return max_x - min_x; - } - - double - height() const - { - return max_y - min_y; - } - }; - - std::ostream& - operator<<(std::ostream& os, const Bounds& b); - } -} - -#endif diff --git a/src/Simulators/Environment/QuadTree.cpp b/src/Simulators/Environment/QuadTree.cpp deleted file mode 100644 index aaa3d6a664..0000000000 --- a/src/Simulators/Environment/QuadTree.cpp +++ /dev/null @@ -1,296 +0,0 @@ -//*************************************************************************** -// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * -// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * -//*************************************************************************** -// This file is part of DUNE: Unified Navigation Environment. * -// * -// Commercial Licence Usage * -// Licencees holding valid commercial DUNE licences may use this file in * -// accordance with the commercial licence agreement provided with the * -// Software or, alternatively, in accordance with the terms contained in a * -// written agreement between you and Faculdade de Engenharia da * -// Universidade do Porto. For licensing terms, conditions, and further * -// information contact lsts@fe.up.pt. * -// * -// Modified European Union Public Licence - EUPL v.1.1 Usage * -// Alternatively, this file may be used under the terms of the Modified * -// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * -// included in the packaging of this file. You may not use this work * -// except in compliance with the Licence. Unless required by applicable * -// law or agreed to in writing, software distributed under the Licence is * -// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * -// ANY KIND, either express or implied. See the Licence for the specific * -// language governing permissions and limitations at * -// https://github.com/LSTS/dune/blob/master/LICENCE.md and * -// http://ec.europa.eu/idabc/eupl.html. * -//*************************************************************************** -// Author: Eduardo Marques * -//*************************************************************************** - -// ISO C++ 98 headers. -#include -#include - -// DUNE headers. -#include - -// Local headers. -#include "QuadTree.hpp" - -namespace Simulators -{ - namespace Environment - { - // The actual implementation - struct Node - { - union - { - QuadTree::Item item; - Node* children[4]; - } m_data; - - bool m_leaf; - - Node(const QuadTree::Item& item) - { - m_data.item = item; - m_leaf = true; - } - - ~Node(void) - { - if (!m_leaf) - { - for (int i = 0; i < 4; ++i) - { - if (m_data.children[i]) - delete m_data.children[i]; - } - } - } - - void - insert(const QuadTree::Item& item, Bounds& b) - { - if (m_leaf) - { - m_leaf = false; - QuadTree::Item prev_item = m_data.item; - std::memset(m_data.children, 0, sizeof(m_data.children)); - insert(prev_item, b); // Split - } - std::pair bq = b.quadrant(item); - Node** c = m_data.children + bq.first; - if (*c) - (*c)->insert(item, bq.second); // Branch - else - *c = new Node(item); // New leaf - } - - bool - isLeaf(void) const - { - return m_leaf; - } - - bool - remove(const Bounds& area, const Bounds& b) - { - if (m_leaf) - return area.contains(m_data.item); - - int cdel = 0; - - for (int i = 0; i < 4; ++i) - { - Node** c = m_data.children + i; - - if (!*c) - continue; - - Bounds cb = b.quadrant(i); - - if (area.intersects(cb) && (*c)->remove(area, cb)) - { - delete *c; - * c = 0; - ++cdel; - } - } - return cdel == 4; // true if all sub-nodes were removed - } - - void - iterate(QuadTree::Iteration& iter, const Bounds& area, const Bounds& b) const - { - if (m_leaf && b.contains(m_data.item)) - iter.process(m_data.item); - else - for (int i = 0; i < 4; ++i) - { - if (m_data.children[i]) - { - Bounds cb = b.quadrant(i); - if (area.intersects(cb)) - m_data.children[i]->iterate(iter, area, cb); - } - } - } - }; - - QuadTree::QuadTree(const Bounds& bounds): - m_bounds(bounds), m_root(0) - { } - - QuadTree::~QuadTree() - { - clear(); - } - - void - QuadTree::iterate(Iteration& iter) const - { - iterate(iter, m_bounds); - } - - void - QuadTree::iterate(Iteration& iter, const Bounds& area) const - { - if (!m_root) - return; - - m_root->iterate(iter, area, m_bounds); - } - - void - QuadTree::clear() - { - if (m_root) - { - delete m_root; - m_root = 0; - } - } - - void - QuadTree::remove(const Bounds& area) - { - if (m_root && m_root->remove(area, m_bounds)) - clear(); // entire contents were deleted - } - - bool - QuadTree::insert(const QuadTree::Item& item) - { - if (!m_bounds.contains(item)) - return false; - - if (!m_root) - m_root = new Node(item); - else - m_root->insert(item, m_bounds); - - return true; - } - - bool - QuadTree::search(const Bounds& search_area, std::vector& v) const - { - class Search: public QuadTree::Iteration - { - public: - Search(std::vector& vector): m_vector(vector) { } - - ~Search(){ } - - void - process(const QuadTree::Item& item) - { - m_vector.push_back(item); - } - - private: - std::vector& m_vector; - }; - - v.clear(); - Search iter(v); - - if (m_root) - m_root->iterate(iter, search_area, m_bounds); - - return v.size() != 0; - } - - uint32_t - QuadTree::size(const Bounds& area) const - { - class count: public QuadTree::Iteration - { - public: - count(): m_elems(0) { } - - ~count(){ } - - void - process(const QuadTree::Item& item) - { - (void)item; - ++m_elems; - } - - uint32_t - result(void) - { - return m_elems; - } - - private: - uint32_t m_elems; - }; - - count iter; - iterate(iter, area); - return iter.result(); - } - - class Dump: public QuadTree::Iteration - { - public: - Dump(std::ostream& stream): - m_stream(stream) - { } - - ~Dump(void) - { } - - void - process(const QuadTree::Item& item) - { - m_stream << item.x << ' ' << item.y << ' ' << item.value << std::endl; - } - - private: - std::ostream& m_stream; - }; - - std::ostream& - operator<<(std::ostream& os, const QuadTree& tree) - { - Dump iter(os); - tree.iterate(iter); - return os; - } - - std::ostream& - operator<<(std::ostream& os, const Bounds& b) - { - return - os << std::fixed << std::setprecision(2) << - b.min_x << ' ' << b.max_x << " | " << - b.min_y << ' ' << b.max_y << " | " << - b.width() << " x " << b.height(); - } - } -} diff --git a/src/Simulators/Environment/Task.cpp b/src/Simulators/Environment/Task.cpp index 11a6ad09e0..af2d783870 100644 --- a/src/Simulators/Environment/Task.cpp +++ b/src/Simulators/Environment/Task.cpp @@ -34,9 +34,6 @@ // DUNE headers. #include -// Local headers. -#include "QuadTree.hpp" - namespace Simulators { //! This task simulates signals for the bottom and forward looking echo sounders @@ -48,6 +45,8 @@ namespace Simulators using DUNE_NAMESPACES; using std::sin; using std::cos; + // QuadTree specific namespace + using namespace Quad; //! Number of points in the forward direction for bottom intersection static const unsigned c_forward_points = 10; diff --git a/src/Simulators/StreamSpeed/HDF5Reader.cpp b/src/Simulators/StreamSpeed/HDF5Reader.cpp deleted file mode 100644 index c600e77983..0000000000 --- a/src/Simulators/StreamSpeed/HDF5Reader.cpp +++ /dev/null @@ -1,218 +0,0 @@ -//*************************************************************************** -// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * -// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * -//*************************************************************************** -// This file is part of DUNE: Unified Navigation Environment. * -// * -// Commercial Licence Usage * -// Licencees holding valid commercial DUNE licences may use this file in * -// accordance with the commercial licence agreement provided with the * -// Software or, alternatively, in accordance with the terms contained in a * -// written agreement between you and Faculdade de Engenharia da * -// Universidade do Porto. For licensing terms, conditions, and further * -// information contact lsts@fe.up.pt. * -// * -// Modified European Union Public Licence - EUPL v.1.1 Usage * -// Alternatively, this file may be used under the terms of the Modified * -// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * -// included in the packaging of this file. You may not use this work * -// except in compliance with the Licence. Unless required by applicable * -// law or agreed to in writing, software distributed under the Licence is * -// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * -// ANY KIND, either express or implied. See the Licence for the specific * -// language governing permissions and limitations at * -// https://github.com/LSTS/dune/blob/master/LICENCE.md and * -// http://ec.europa.eu/idabc/eupl.html. * -//*************************************************************************** -// Author: Miguel Aguiar * -//*************************************************************************** - -#include -#include - -#include "DUNE/I18N.hpp" - -// This component depends on the h5cpp library -// (https://github.com/ess-dmsc/h5cpp.com). -// To enable support for h5cpp, set the H5CPP flag to 1 when calling CMake. -// If support for h5cpp is not enabled, the compilation will not fail, but -// the constructor of HDF5Reader will throw. -#if DUNE_H5CPP_ENABLED -# include "h5cpp/hdf5.hpp" -#else -// Define hdf5::file::File to avoid compilation errors due to incomplete type -namespace hdf5 -{ - namespace file - { - class File - {}; - } // namespace file -} // namespace hdf5 -#endif - -#include "HDF5Reader.hpp" - -namespace Simulators -{ - namespace StreamSpeed - { - namespace StreamGenerator - { - HDF5Reader::HDF5Reader(std::string const& filename) - : m_file(std::make_unique()) - { -#ifndef DUNE_H5CPP_ENABLED - throw std::runtime_error( - DTR("HDF5Reader::HDF5Reader(): support for HDF5 i/o is not " - "enabled.")); -#else - try - { - *m_file = - hdf5::file::open(filename, hdf5::file::AccessFlags::READONLY); - - if (m_file == nullptr) - throw; - } - catch (...) - { - throw std::runtime_error( - DTR("HDF5Reader::HDF5Reader(): unable to open file.")); - } -#endif - } - - HDF5Reader::~HDF5Reader() = default; - -#ifdef DUNE_H5CPP_ENABLED - inline bool - dsetExists(File const* f, std::string const& path) - { - return f->root().has_dataset(path); - return false; - } -#endif - - bool - HDF5Reader::datasetExists(std::string const& path) const - { -#ifdef DUNE_H5CPP_ENABLED - return dsetExists(m_file.get(), path); -#else - return false; -#endif - } - - template - HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const& path) const - { -#ifdef DUNE_H5CPP_ENABLED - if (!dsetExists(m_file.get(), path)) - throw std::runtime_error( - DTR("HDF5Reader::getDataset(): The requested dataset does not " - "exist.")); - - auto dset = m_file->root().get_dataset(path); - auto dimensions = - hdf5::dataspace::Simple(dset.dataspace()).current_dimensions(); - - // Total number of gridpoints is the product of the sizes of all - // dimensions. - size_t size = std::accumulate(std::begin(dimensions), - std::end(dimensions), - 1, - std::multiplies<>()); - - std::vector data(size); - dset.read(data); - - // Convert elements of dimensions to size_t - std::vector dimensions_(dimensions.size()); - - std::copy(std::begin(dimensions), - std::end(dimensions), - std::begin(dimensions_)); - - return {dimensions_, data}; -#else - return {}; -#endif - } - - template - std::vector - HDF5Reader::getAttribute(std::string const& path, - std::string const& attribute) const - { -#ifdef DUNE_H5CPP_ENABLED - auto node = hdf5::node::get_node(m_file->root(), path); - auto attributes = node.attributes; - - if (!attributes.exists(attribute)) - throw std::runtime_error( - DTR("HDF5Reader::getAttribute(): requested attribute does not " - "exist.")); - - auto attr_handle = attributes[attribute]; - - std::vector data(attr_handle.dataspace().size()); - - attr_handle.read(data); - - return data; -#else - return {}; -#endif - } - - // Template specialization declarations for getDataset - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - // Template specialization declarations for getAttribute - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - } // namespace StreamGenerator - } // namespace StreamSpeed -} // namespace Simulators diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp index 31df4c100f..7faffe55c6 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp +++ b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp @@ -34,7 +34,7 @@ #include #include "Grid.hpp" -#include "HDF5Reader.hpp" +#include "DUNE/Parsers/HDF5Reader.hpp" #include "StreamGenerator.hpp" namespace Simulators @@ -91,7 +91,7 @@ namespace Simulators double time = 0.0) const override; private: - HDF5Reader m_file; + DUNE::Parsers::HDF5Reader m_file; //! Velocity in the East direction. std::vector m_u; //! Velocity in the North direction.