comparison python/prediction.py @ 571:a9c1d61a89b4

corrected bug for segment intersection
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 07 Aug 2014 00:05:14 -0400
parents 806df5f61c03
children 84690dfe5560
comparison
equal deleted inserted replaced
570:5adaab9ad160 571:a9c1d61a89b4
164 t2 = 0 164 t2 = 0
165 while not cz and t2 < timeHorizon: 165 while not cz and t2 < timeHorizon:
166 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: 166 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
167 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) 167 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
168 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) 168 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
169 if cz: 169 if cz != None:
170 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) 170 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
171 t2 += 1 171 t2 += 1
172 t1 += 1 172 t1 += 1
173 173
174 if debug: 174 if debug: