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