Source code for tracklib.util.Geometry

# -*- coding: utf-8 -*-
"""Definition of some geometric functions"""
# -------------------------- Geometry Functions -------------------------------
# Geometry Functions  like:
#    - cartesienne
#    - __eval
#    - __right

#    - dist_point_droite
#    - dist_point_to_segment
#    - distance_to_segment

#    - projection_droite
#    - proj_segment
#    - proj_polyligne

#    - triangle_area
#    - aire_visval

#    - isSegmentIntersects
#    - intersection
#    - intersects

#    - inclusion

#    - transform
#    - transform_inverse
#
#    - angleBetweenThreePoints
#    -
# ----------------------------------------------------------------

# For type annotation
from __future__ import annotations   

import math
import numpy as np

# --------------------------------------------------------------------------
# Circular import (not satisfying solution)
# --------------------------------------------------------------------------
from tracklib.core.Obs import Obs
from tracklib.core.ObsTime import ObsTime
import tracklib.core.Utils as Utils


[docs]def cartesienne(segment) -> list[float, float, float]: """Fonction equation cartesienne :return: liste de paramètres """ parametres = list() x1 = segment[0] y1 = segment[1] x2 = segment[2] y2 = segment[3] u1 = x2 - x1 u2 = y2 - y1 b = -u1 a = u2 c = -(a * x1 + b * y1) parametres.append(a) parametres.append(b) parametres.append(c) return parametres
# ---------------------------------------- # Fonction de test d'equation de droite # Entrees : paramatres et coords (x,y) # Sortie : en particulier 0 si le point # appartient a la droite # ----------------------------------------
[docs]def __eval(param, x, y): a = param[0] b = param[1] c = param[2] return a * x + b * y + c
[docs]def direction(a, b, c): return (c[0] - a[0]) * (b[1] - a[1]) - (b[0] - a[0]) * (c[1] - a[1])
# ---------------------------------------- # Fonction distance point-droite # Entree : paramètres a,b,c d'une droite, # coordonnées x et y du point # Sortie : distance du point à la droite # ----------------------------------------
[docs]def dist_point_droite(param, x, y): a = param[0] b = param[1] c = param[2] distance = math.fabs(a * x + b * y + c) distance /= math.sqrt(a * a + b * b) return distance
[docs]def dist_point_to_segment(point, segment): return dist_point_droite(cartesienne(segment), point.getX(), point.getY())
[docs]def distance_to_segment(x0: float, y0: float, x1: float, y1: float, x2: float, y2: float) -> float: """Function to compute distance between a point and a segment :param x0: point coordinate X :param y0: point coordinate Y :param x1: segment first point X :param y1: segment first point Y :param x2: segment second point X :param y2: segment second point Y :return: Distance between point and projection coordinates """ # Segment length l = math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) # Normalized scalar product psn = ((x0 - x1) * (x2 - x1) + (y0 - y1) * (y2 - y1)) / l X = max(x1, x2) Y = max(y1, y2) x = min(x1, x2) y = min(y1, y2) xproj = x1 + psn / l * (x2 - x1) yproj = y1 + psn / l * (y2 - y1) xproj = min(max(xproj, x), X) yproj = min(max(yproj, y), Y) # Compute distance d = math.sqrt((x0 - xproj) * (x0 - xproj) + (y0 - yproj) * (y0 - yproj)) return d
[docs]def projection_droite(param: tuple[float, float, float], x: float, y: float) -> tuple[float, float]: """ Fonction projection orthogonale sur une droite :param param: Paramètres a,b,c d'une droite :param x: X du point à projeter :param y: Y du point à projeter :return: coordonnée xproj et yproj du point projeté """ a = param[0] b = param[1] c = param[2] if b == 0: return (x, a) xv = -b yv = a norm = math.sqrt(xv * xv + yv * yv) xb = 0 yb = -c / b BH = ((x - xb) * xv + (y - yb) * yv) / norm xproj = xb + BH * xv / norm yproj = yb + BH * yv / norm return xproj, yproj
# ---------------------------------------- # Fonction complète de projection # Entree : segment, coordonnées x et y du # point # Sortie : distance du point au segment et # coordonnées du projeté # ----------------------------------------
[docs]def proj_segment(segment, x, y): param = cartesienne(segment) a = param[0] b = param[1] c = param[2] distance = math.fabs(a * x + b * y + c) distance /= math.sqrt(a * a + b * b) # Récupération des coordonnées du projeté xproj, yproj = projection_droite(param, x, y) # Test d'inclusion dans le segment x1 = segment[0] y1 = segment[1] x2 = segment[2] y2 = segment[3] boolx1 = (xproj >= x1) and (xproj <= x2) boolx2 = (xproj <= x1) and (xproj >= x2) boolx = boolx1 or boolx2 booly1 = (yproj >= y1) & (yproj <= y2) booly2 = (yproj <= y1) & (yproj >= y2) booly = booly1 or booly2 bool_include = boolx and booly # Si le projeté est dans le segment if bool_include: a = param[0] b = param[1] c = param[2] xv = -b yv = a norm = math.sqrt(xv * xv + yv * yv) xb = 0 yb = -c / b BH = ((x - xb) * xv + (y - yb) * yv) / norm xproj = xb + BH * xv / norm yproj = yb + BH * yv / norm return distance, xproj, yproj else: distance1 = math.sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1)) distance2 = math.sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2)) if distance1 <= distance2: return distance1, x1, y1 else: return distance2, x2, y2
# ---------------------------------------- # Fonction complète de projection entre un # point et une polyligne # Entree : polyligne, coordonnées x et y du # point # Sortie : distance du point ) la polyligne # et coordonnées du projeté # ----------------------------------------
[docs]def proj_polyligne(Xp, Yp, x, y): distmin = 1e400 for i in range(len(Xp) - 1): x1 = Xp[i] y1 = Yp[i] x2 = Xp[i + 1] y2 = Yp[i + 1] dist, xp, yp = proj_segment([x1, y1, x2, y2], x, y) if dist < distmin: distmin = dist xproj = xp yproj = yp iproj = i return distmin, xproj, yproj, iproj
# -------------------------------------------------------------------------- # Function to compute area of triangle with cross product # -------------------------------------------------------------------------- # Input : # - x0, y0 :: point 1 coordinates # - x1, y1 :: point 2 coordinates # - x2, y2 :: point 3 coordinates # -------------------------------------------------------------------------- # Output : area of P1P2P3 in coordinate units # --------------------------------------------------------------------------
[docs]def triangle_area(x0, y0, x1, y1, x2, y2): return 0.5 * abs((x1 - x0) * (y2 - y1) - (x2 - x1) * (y1 - y0))
# ============================================================================= """ Pour calculer l'aire des triangles dans Visvalingam """
[docs]def aire_visval(track, i): x0 = track.getObs(i - 1).position.getX() y0 = track.getObs(i - 1).position.getY() x1 = track.getObs(i).position.getX() y1 = track.getObs(i).position.getY() x2 = track.getObs(i + 1).position.getX() y2 = track.getObs(i + 1).position.getY() return triangle_area(x0, y0, x1, y1, x2, y2)
# ---------------------------------------- # Fonction booleenne d'intersection # Entrees : segment1 et segment2 # Sortie : true s'il y a intersection # ----------------------------------------
[docs]def isSegmentIntersects(segment1, segment2): param_1 = cartesienne(segment1) param_2 = cartesienne(segment2) # a1 = param_1[0] # b1 = param_1[1] # c1 = param_1[2] # a2 = param_2[0] # b2 = param_2[1] # c2 = param_2[2] x11 = segment1[0] y11 = segment1[1] x12 = segment1[2] y12 = segment1[3] x21 = segment2[0] y21 = segment2[1] x22 = segment2[2] y22 = segment2[3] val11 = __eval(param_1, x21, y21) val12 = __eval(param_1, x22, y22) val21 = __eval(param_2, x11, y11) val22 = __eval(param_2, x12, y12) val1 = val11 * val12 val2 = val21 * val22 return (val1 <= 0) & (val2 <= 0)
# ---------------------------------------- # Intersection between 2 tracks # withTime: time constraint (in secs) # (-1 if no time constraint) # ----------------------------------------
[docs]def intersection(track1, track2, withTime=-1): if not (track1.getSRID() == track2.getSRID()): print("Error: tracks must have same SRID to compute intersections") exit() from tracklib.core.Track import Track I = Track() TMP_I = [] TMP_J = [] TMP_TPS2 = [] for i in range(len(track1) - 1): x11 = track1[i].position.getX() y11 = track1[i].position.getY() x12 = track1[i + 1].position.getX() y12 = track1[i + 1].position.getY() seg1 = [x11, y11, x12, y12] for j in range(len(track2) - 1): x21 = track2[j].position.getX() y21 = track2[j].position.getY() x22 = track2[j + 1].position.getX() y22 = track2[j + 1].position.getY() seg2 = [x21, y21, x22, y22] if isSegmentIntersects(seg1, seg2): P1 = cartesienne(seg1) P2 = cartesienne(seg2) A = np.zeros((2, 2)) B = np.zeros((2, 1)) A[0, 0] = P1[0] A[0, 1] = P1[1] B[0, 0] = -P1[2] A[1, 0] = P2[0] A[1, 1] = P2[1] B[1, 0] = -P2[2] X = np.linalg.solve(A, B) x = X[0, 0] y = X[1, 0] p = Utils.makeCoords(x, y, 0, track1.getSRID()) # Linear interpolation on track 1 w1 = p.distance2DTo(track1[i].position) w2 = p.distance2DTo(track1[i + 1].position) p.setZ( ( w1 * track1[i + 1].position.getZ() + w2 * track1[i].position.getZ() ) / (w1 + w2) ) t1 = track1[i].timestamp.toAbsTime() t2 = track1[i].timestamp.toAbsTime() ta = (w1 * t2 + w2 * t1) / (w1 + w2) # Linear interpolation on track 2 w1 = p.distance2DTo(track2[j].position) w2 = p.distance2DTo(track2[j + 1].position) t1 = track2[i].timestamp.toAbsTime() t2 = track2[i].timestamp.toAbsTime() tb = (w1 * t2 + w2 * t1) / (w1 + w2) # Add intersection if (withTime == -1) or (abs(tb - ta) < withTime): I.addObs(Obs(p, ObsTime.readUnixTime(ta))) TMP_TPS2.append(ObsTime.readUnixTime(tb)) TMP_I.append(i) TMP_J.append(j) if I.size() > 0: I.createAnalyticalFeature("timestamp2", TMP_TPS2) I.createAnalyticalFeature("id1", TMP_I) I.createAnalyticalFeature("id2", TMP_J) return I
# ---------------------------------------- # Intersection between 2 tracks (boolean) # ----------------------------------------
[docs]def intersects(track1, track2): for i in range(len(track1) - 1): x11 = track1[i].position.getX() y11 = track1[i].position.getY() x12 = track1[i + 1].position.getX() y12 = track1[i + 1].position.getY() seg1 = [x11, y11, x12, y12] for j in range(len(track2) - 1): x21 = track2[j].position.getX() y21 = track2[j].position.getY() x22 = track2[j + 1].position.getX() y22 = track2[j + 1].position.getY() seg2 = [x21, y21, x22, y22] if isSegmentIntersects(seg1, seg2): return True return False
# -------------------------------------------------------- # Fonction booleenne d'inlcusion d'un pt dans un polygone # Un polygone est defini comme une liste de points : # --------------------------------------------------------
[docs]def inclusion(X, Y, x, y): cmax = 2 * max(np.max(X), np.max(Y)) segment = list() segment.append(x) segment.append(y) segment.append(cmax) segment.append(cmax) n = 0 # Number of intersections for i in range(len(X) - 1): edge = list() edge.append(X[i]) edge.append(Y[i]) edge.append(X[i + 1]) edge.append(Y[i + 1]) if isSegmentIntersects(segment, edge): n += 1 return n % 2 == 1
[docs]def transform(theta, tx, ty, X, Y): XR = [0] * len(X) YR = [0] * len(Y) ct = math.cos(theta) st = math.sin(theta) for j in range(len(X)): XR[j] = ct * (X[j] - tx) + st * (Y[j] - ty) YR[j] = -st * (X[j] - tx) + ct * (Y[j] - ty) return XR, YR
[docs]def transform_inverse(theta, tx, ty, X, Y): XR = [0] * len(X) YR = [0] * len(Y) ct = math.cos(theta) st = math.sin(theta) for j in range(len(X)): XR[j] = ct * X[j] - st * Y[j] + tx YR[j] = st * X[j] + ct * Y[j] + ty return XR, YR
# Fonction de calcul de l'azimut (en °) # Entrées : coordonnées de P1 et P2 # Sortie : azimut (en °) de P1 vers P2
[docs]def azimut(x1, y1, x2, y2): dx = x2 - x1 dy = y2 - y1 if (dx == 0) and (dy == 0): print("Erreur : les points p1 et p2 doivent être distincts") return 0 q = math.sqrt(dx * dx + dy * dy) + dy if q == 0: azimut = 0 else: azimut = 2 * math.atan(dx / q) * 180 / math.pi if azimut < 0: azimut += 360 return azimut
[docs]def collinear(p1, p2, p3): """ Parameters ---------- p1, p2, p3 : [x, y] Returns ------- bool DESCRIPTION. """ x1 = p1[0] y1 = p1[1] x2 = p2[0] y2 = p2[1] x3 = p3[0] y3 = p3[1] xVectP1P2 = x2 - x1 yVectP1P2 = y2 - y1 xVectP1P3 = x3 - x1 yVectP1P3 = y3 - y1 if xVectP1P2 * yVectP1P3 - xVectP1P3 * yVectP1P2 == 0: # points are collinear return True else: # Not collinear return False
[docs]def angleBetweenThreePoints(o1, o2, o3): """ Compute angle between three points (the angle is calculated for the middle point). :param float o_1: first point :param float o_2: second point :param float o_3: third point :return: angle in radian """ x1 = o1.position.getX() x2 = o2.position.getX() x3 = o3.position.getX() y1 = o1.position.getY() y2 = o2.position.getY() y3 = o3.position.getY() # 3 points confondus if x1 == x2 and x2 == x3 and y1 == y2 and y2 == y3: return 0 # 2 points confondus if x1 == x2 and y1 == y2: return 0 if x1 == x3 and y1 == y3: return 0 if x2 == x3 and y2 == y3: return 0 # print (x1, x2, x3, y1, y2, y3) num = (x1 - x2) * (x3 - x2) + (y1 - y2) * (y3 - y2) den = math.sqrt((x1-x2)**2 + (y1-y2)**2) * math.sqrt((x3-x2)**2 + (y3-y2)**2) r = num / den if r > 1: r = 1 if r < -1: r = -1 return math.acos(r)