def calculateNextPoints(self):
id_ = {}
list_ = []
self.feedListPt(list_)
for p in list_:
contained_ = self.isContainedHull(p)
all_ = {}
allKeys_ = []
self.feedMapping(p, all_, allKeys_)
next_ = all_.keys()
first_ = allKeys_[0]
once_ = []
for k, v in all_.items():
if v != IndexConstants.ONE_ELEMENT:
continue
once_.append(k)
if len(once_) > 0 and contained_:
eOne_ = once_[0]
eTwo_ = once_[len(once_) - 1]
v_ = Vecteur(p, eTwo_)
sOne_ = SitePoint(eOne_, p, v_)
v_ = Vecteur(p, eOne_)
sTwo_ = SitePoint(eTwo_, p, v_)
if SiteInfo.compare(sOne_,
sTwo_) == TriConstantes.NO_SWAP_SORT:
first_ = eTwo_
else:
first_ = eOne_
Delaunay.putPts(id_, p, next_, first_)
return id_
def distance(vecteur1, vecteur2):
vect1 = Vecteur.normalize(vecteur1)
vect2 = Vecteur.normalize(vecteur2)
dist = 0
for i in range(len(vecteur1)):
# pas de division par zero..
if vect1[i] == 0.0 and vect2[i] == 0.0:
continue
dist += (((vect1[i] - vect2[i])**2) / (vect1[i] + vect2[i])) / 2
return dist
def __init__(self, _point, _first, _vect):
self.point = _point
vect_ = Vecteur(_first, _point)
det_ = _vect.det(vect_)
scal_ = _vect.prodScal(vect_)
self.info = SiteInfo()
if (scal_ >= 0 and det_ <= 0):
self.info.nb = (SiteInfo.QUAD_ONE)
elif (scal_ < 0 and det_ <= 0):
self.info.nb = (SiteInfo.QUAD_TWO)
elif (scal_ <= 0):
#det_ < 0
self.info.nb = (SiteInfo.QUAD_THREE)
else:
self.info.nb = (SiteInfo.QUAD_FOUR)
self.info.norme = vect_.normeCarree()
self.info.scal = scal_
def getSites(_points, _p, _others, _next, _nextOthOne, _nextOthTwo) :
o_ = _points[_next]
_others.append(_points[_nextOthOne])
_others.append(_points[_nextOthTwo])
sites_ = []
v_ = Vecteur(_p, o_);
for n in _others :
sites_.append(SitePoint(n, _p, v_));
return sorted(sites_,key=cmp_to_key(SiteInfo.compare))
def putPts(_id, _p, _next, _first):
sites_ = []
v_ = Vecteur(_p, _first)
for n in _next:
sites_.append(SitePoint(n, _p, v_))
pts_ = []
for s in sorted(sites_, key=cmp_to_key(SiteInfo.compare)):
pts_.append(s.point)
_id[_p] = pts_
def getConvexHull(self,_sortie) :
cust_ = self.pts[0]
for p in self.pts :
if Polygone.hasToRedef(cust_, p) :
cust_ = p
endPoint_ = self.pts[0]
nbVertices_ = len(self.pts);
while len(_sortie.pts) == 0 or endPoint_ is not _sortie.pts[0] :
_sortie.pts.append(cust_)
endPoint_ = self.pts[0]
for j in range(nbVertices_):
if endPoint_ is cust_ :
endPoint_ = self.pts[j]
else :
b_ = _sortie.pts[len(_sortie.pts)-1];
affineSegment_ = Vecteur(b_, endPoint_)
affinePoint_ = Vecteur(self.pts[j], endPoint_)
currentSide_ = Polygone.getSide(affineSegment_, affinePoint_)
if currentSide_ == LinearDirection.LEFT :
endPoint_ = self.pts[j]
cust_ = endPoint_
return _sortie
def containsPoint(self, _c) :
one_ = Vecteur(self.x, _c)
two_ = Vecteur(self.y, _c)
return one_.det(two_) == 0 and one_.scal(two_) <= 0
def tryAddTriangle(self, _first, _second, _third):
vOne_ = Vecteur(_first, _second)
vTwo_ = Vecteur(_first, _third)
if vOne_.det(vTwo_) == 0:
return None
self.triangles.append(Triangle(_first, _second, _third))
def asservissement(erreur, rep):
pos, vit = rep
return 1 * erreur - 10 * vit[-1]
#cibles = [Vecteur(rd.randint(0, 200) / 100, rd.randint(0, 200) / 100) for _ in range(15)]
#cibles =[Vecteur(1), Vecteur(1, 1)]
cibles = []
with open('trajet_prof.csv', 'r') as file:
datas = file.readlines()
for line in datas[1:]:
line = line.split(',')
cibles.append(Vecteur(float(line[1]), float(line[2])))
pos_initial = Vecteur()
pos = [pos_initial]
vit = [Vecteur()]
try:
for cible in cibles:
aller_a(cible, (pos, vit), (0.01, 1))
except KeyboardInterrupt:
pass
aller_a(Vecteur(), (pos, vit), (0.001, 0.001), True)
print(len(pos))
from matplotlib import pyplot as plt
plt.plot([elem.x for elem in pos], [elem.y for elem in pos], '-k')