def rotate(self, rotation_angle, origin=None): """ Rotates the position value by the specified angle using a standard 2D rotation matrix formulation. If an origin Position2D instance is not specified the rotation will occur around the origin. Also, if an origin is specified, the uncertainty in that origin value will be propagated through to the uncertainty of the rotated result. :param rotation_angle: :param origin: (optional) :return: """ if origin is None: origin = self.__class__(value.ValueUncertainty(0, 0), value.ValueUncertainty(0, 0)) a = rotation_angle.radians x = self.x.raw - origin.x.raw y = self.y.raw - origin.y.raw self.x.update( x * math.cos(a) - y * math.sin(a) + origin.x.raw, ops.sqrt_sum_of_squares(self.x.uncertainty, origin.x.uncertainty), ) self.y.update( y * math.cos(a) + x * math.sin(a) + origin.y.raw, ops.sqrt_sum_of_squares(self.y.uncertainty, origin.y.uncertainty), ) return self
def rotate(self, rotation_angle, origin=None): """ Rotates the position value by the specified angle using a standard 2D rotation matrix formulation. If an origin Position2D instance is not specified the rotation will occur around the origin. Also, if an origin is specified, the uncertainty in that origin value will be propagated through to the uncertainty of the rotated result. :param rotation_angle: :param origin: (optional) :return: """ if origin is None: origin = self.__class__( value.ValueUncertainty(0, 0), value.ValueUncertainty(0, 0) ) a = rotation_angle.radians x = self.x.raw - origin.x.raw y = self.y.raw - origin.y.raw self.x.update( x * math.cos(a) - y*math.sin(a) + origin.x.raw, ops.sqrt_sum_of_squares(self.x.uncertainty, origin.x.uncertainty) ) self.y.update( y * math.cos(a) + x*math.sin(a) + origin.y.raw, ops.sqrt_sum_of_squares(self.y.uncertainty, origin.y.uncertainty) ) return self
def closest_point_on_line(point, line_start, line_end, contained=True): """ Finds the closest point on a line to the specified point using the formulae discussed in the "another formula" section of: wikipedia.org/wiki/Distance_from_a_point_to_a_line#Another_formula """ length = line_start.distance_from(line_end) if not length: raise ValueError("Cannot calculate point. Invalid line segment.") s = line_start e = line_end delta_x = e.x.raw - s.x.raw delta_y = e.y.raw - s.y.raw rotate = False slope = 0.0 slope_unc = 0.0 try: slope = delta_y / delta_x slope_unc = abs(1.0 / delta_x) * (s.y.raw_uncertainty + e.y.raw_uncertainty) + abs(slope / delta_x) * ( s.x.raw_uncertainty + e.x.raw_uncertainty ) except Exception: rotate = True raise if rotate or (abs(slope) > 1.0 and abs(slope_unc / slope) > 0.5): a = angle.Angle(degrees=20.0) e2 = e.clone().rotate(a, s) p2 = point.clone().rotate(a, s) print(point, p2) print(e, e2) result = closest_point_on_line(p2, s, e2, contained) if result is None: return result a.degrees = -20.0 result.rotate(a, s) return result intercept = s.y.raw - slope * s.x.raw denom = slope * slope + 1.0 numer = point.x.raw + slope * (point.y.raw - intercept) x = numer / denom y = (slope * numer) / denom + intercept if contained: # Check to see if point is between start and end values x_range = sorted([s.x.raw, e.x.raw]) y_range = sorted([s.y.raw, e.y.raw]) eps = 1e-8 x_min = x - eps x_max = x + eps y_min = y - eps y_max = y + eps out_of_bounds = x_range[1] < x_min or x_max < x_range[0] or y_range[1] < y_min or y_max < y_range[0] if out_of_bounds: return None start_dist = ops.sqrt_sum_of_squares(s.x.raw - x, s.y.raw - y) end_dist = ops.sqrt_sum_of_squares(e.x.raw - x, e.y.raw - y) x_unc = start_dist / length.raw * s.x.raw_uncertainty + end_dist / length.raw * e.x.raw_uncertainty x_unc = math.sqrt(x_unc ** 2 + point.x.raw_uncertainty ** 2) y_unc = start_dist / length.raw * s.y.raw_uncertainty + end_dist / length.raw * e.y.raw_uncertainty y_unc = math.sqrt(y_unc ** 2 + point.y.raw_uncertainty ** 2) return create_point(x=x, y=y, x_unc=x_unc, y_unc=y_unc)
def closest_point_on_line(point, line_start, line_end, contained=True): """ Finds the closest point on a line to the specified point using the formulae discussed in the "another formula" section of: wikipedia.org/wiki/Distance_from_a_point_to_a_line#Another_formula """ length = line_start.distance_from(line_end) if not length: raise ValueError('Cannot calculate point. Invalid line segment.') s = line_start e = line_end delta_x = e.x.raw - s.x.raw delta_y = e.y.raw - s.y.raw rotate = False slope = 0.0 slope_unc = 0.0 try: slope = delta_y / delta_x slope_unc = ( abs(1.0 / delta_x) * ( s.y.raw_uncertainty + e.y.raw_uncertainty ) + abs( slope / delta_x ) * ( s.x.raw_uncertainty + e.x.raw_uncertainty ) ) except Exception: rotate = True raise if rotate or (abs(slope) > 1.0 and abs(slope_unc / slope) > 0.5): a = angle.Angle(degrees=20.0) e2 = e.clone().rotate(a, s) p2 = point.clone().rotate(a, s) print(point, p2) print(e, e2) result = closest_point_on_line(p2, s, e2, contained) if result is None: return result a.degrees = -20.0 result.rotate(a, s) return result intercept = s.y.raw - slope * s.x.raw denom = slope * slope + 1.0 numer = point.x.raw + slope * (point.y.raw - intercept) x = numer / denom y = (slope * numer) / denom + intercept if contained: # Check to see if point is between start and end values x_range = sorted([s.x.raw, e.x.raw]) y_range = sorted([s.y.raw, e.y.raw]) eps = 1e-8 x_min = x - eps x_max = x + eps y_min = y - eps y_max = y + eps out_of_bounds = ( x_range[1] < x_min or x_max < x_range[0] or y_range[1] < y_min or y_max < y_range[0] ) if out_of_bounds: return None start_dist = ops.sqrt_sum_of_squares(s.x.raw - x, s.y.raw - y) end_dist = ops.sqrt_sum_of_squares(e.x.raw - x, e.y.raw - y) x_unc = ( start_dist / length.raw * s.x.raw_uncertainty + end_dist / length.raw * e.x.raw_uncertainty ) x_unc = math.sqrt(x_unc ** 2 + point.x.raw_uncertainty ** 2) y_unc = ( start_dist / length.raw * s.y.raw_uncertainty + end_dist / length.raw * e.y.raw_uncertainty ) y_unc = math.sqrt(y_unc ** 2 + point.y.raw_uncertainty ** 2) return create_point(x=x, y=y, x_unc=x_unc, y_unc=y_unc)
def test_sqrtSumOfSquares(self): """test_sqrtSumOfSquares doc...""" self.assertEqual(1.0, ops.sqrt_sum_of_squares(-1.0)) self.assertEqual(math.sqrt(2), ops.sqrt_sum_of_squares(1.0, 1.0)) self.assertEqual(math.sqrt(4.25), ops.sqrt_sum_of_squares(2.0, 0.5))