track_segmentation.py 文件源码

def __init__(self,x1,y1,x2,y2,x3,y3,sector,endpoint):
    self.x_m=x1; self.x=x2; self.x_p=x3; self.y_m=y1; self.y=y2; self.y_p=y3;
    self.length_m = math.hypot(self.x_m-self.x, self.y_m-self.y)
    self.length_p = math.hypot(self.x_p-self.x, self.y_p-self.y)
    self.length_secant = math.hypot(self.x_p-self.x_m, self.y_p-self.y_m)
    self.length = (self.length_m+self.length_p)/2
    self.sector = sector;

    if endpoint:
      self.curvature = 0
    else:
      p = (self.length_m+self.length_p+self.length_secant)/2
      #print(p, self.length_m, self.length_p, self.length_secant)
      try:
        area = math.sqrt(p*(p-self.length_m)*(p-self.length_p)*(p-self.length_secant))
      except ValueError:
        area=0

      self.curvature = 4*area/(self.length_m*self.length_p*self.length_secant)