def _project_point_onto_plane(equation, point):
"""Calculate coordinates of a point perpendicularly projected onto plane
Parameters
----------
equation : List[float]
A list of coefficients of the equation describing the plane :math:`Ax +
By + Cz + D = 0`. The length should hence be 4. For example, the
plane :math:`z = 0` corresponds to the argument ``[0, 0, 1, 0]``.
point : List[float]
The coordinates of the point ``[x, y, z]``. A list of length 3.
Returns
-------
np.ndarray[float]
The coordinates of the given point projected perpendicularly to the
given plane. Calculated according to equation:
.. math::
\\vec{OA'} = \\vec{OA} - d \\frac{\mathbf{n}}{\|\mathbf{n}\|},
where :math:`\mathbf{n}` is the vector normal to the plane.
"""
normal = np.array(equation[:3])
point = np.array(point)
return point - _plane_point_dist(equation,
point) * normal / vec_norm(normal)
def _plane_point_dist(equation, point):
"""Calculate the distance between a point and a plane given by equation
Parameters
----------
equation : List[float]
A list of coefficients of the equation describing the plane :math:`Ax +
By + Cz + D = 0`. The length should hence be 4. For example, the
plane :math:`z = 0` corresponds to the argument ``[0, 0, 1, 0]``.
point : List[float]
The coordinates of the point ``[x, y, z]``. A list of length 3.
Returns
-------
float
The calculated distance according to the equation:
.. math::
d = \\frac{A x + B y + C z + D}{\sqrt{A^2 + B^2 + C^2}}
Returning the signed value of this expression allows to distinguish
between points lying on the opposite sides of the plane.
"""
normal = np.array(equation[:3])
point = np.array(point)
return (np.dot(normal, point) + equation[3]) / vec_norm(normal)
def get_equal_prob(self, current_loc, person, timestamp):
prev_loc_1 = person.get_closest_loc(timestamp, 1)
prev_loc_2 = person.get_closest_loc(timestamp, 2)
if prev_loc_1 != prev_loc_2:
prev_mov_vec = prev_loc_2 - prev_loc_1
cur_mov_vec = prev_loc_1 - current_loc
prev_dist = vec_norm(prev_mov_vec)
cur_dist = vec_norm(cur_mov_vec)
angle_diff = math.acos(
(prev_mov_vec @ cur_mov_vec) / (prev_dist * cur_dist))
p_dst = self.distance_prob(cur_dist, prev_dist)
p_angle = self.angle_prob(angle_diff)
fac = self.angle_dist_fac(cur_dist)
return (1 - fac) * p_dst + fac * p_angle
else:
dist = current_loc - prev_loc_1
return
