def getLineSegmentIntersection(line1, line2):
if linesAreParallel(line1, line2):
return None
A, B = line1
C, D = line2
bVector = calculate.subtractPoints(B, A)
dVector = calculate.subtractPoints(D, C)
cVector = calculate.subtractPoints(C, A)
bperp = vector.getRightPerpendicular(bVector)
dperp = vector.getRightPerpendicular(dVector)
dperpDotB = calculate.dotProduct(dperp,
bVector)
dperpDotC = calculate.dotProduct(dperp,
cVector)
bperpDotC = calculate.dotProduct(bperp,
cVector)
distanceAlongB = float(dperpDotC) / float(dperpDotB)
distanceAlongD = float(bperpDotC) / float(dperpDotB)
if (distanceAlongB > 0 and distanceAlongB < 1 and
distanceAlongD > 0 and distanceAlongD < 1):
AToIntersectionPoint = calculate.multiplyVectorAndScalar(bVector,
distanceAlongB)
intersectionPoint = calculate.addPointAndVector(A,
AToIntersectionPoint)
return intersectionPoint
else:
return None
def linesAreParallel(line1, line2):
A, B = line1
C, D = line2
ab_vector = calculate.subtractPoints(B, A)
cd_vector = calculate.subtractPoints(D, C)
cd_perp = vector.getRightPerpendicular(cd_vector)
cdperp_dot_ab = calculate.dotProduct(
cd_perp,
ab_vector)
return cdperp_dot_ab == 0
def avoidwalls(agent,
walls):
if not walls:
return (0, 0)
agentPosition = agent.getPosition()
feelers = createFeelers(agent)
closestWall = None
closestIntersection = None
distanceSquaredToClosestIntersection = None
closestFeeler = None
for feeler in feelers:
for wall in walls:
intersectPoint = intersect.lineWithLine(feeler,
wall)
if intersectPoint is None:
continue
agentToIntersection = calculate.subtractPoints(intersectPoint,
agentPosition)
distanceSquaredToIntersection = vector.getMagnitudeSquared(agentToIntersection)
if closestIntersection is None or distanceSquaredToIntersection < distanceSquaredToClosestIntersection:
distanceSquaredToClosestIntersection = distanceSquaredToIntersection
closestWall = wall
closestIntersection = intersectPoint
closestFeeler = feeler
if closestWall is None:
return (0, 0)
(closestFeelerOrigin,
closestFeelerEndpoint) = closestFeeler
(wallOrigin,
wallEndpoint) = closestWall
wallVector = calculate.subtractPoints(wallEndpoint,
wallOrigin)
intersectionToFeelerEndpoint = calculate.subtractPoints(closestFeelerEndpoint,
closestIntersection)
overshootLength = vector.getMagnitude(intersectionToFeelerEndpoint)
normalizedWallVector = vector.normalize(wallVector)
wallNormal = vector.getRightPerpendicular(normalizedWallVector)
steeringForce = calculate.multiplyVectorAndScalar(wallNormal,
overshootLength)
return steeringForce
def test_diagonal_xBecomesNegativeY_yBecomesPositiveX(self):
startVector = (1, 2)
perpVector = vector.getRightPerpendicular(startVector)
self.assertEquals((2, -1),
perpVector)
def test_negativeY_positiveX(self):
startVector = (0, 1)
perpVector = vector.getRightPerpendicular(startVector)
self.assertEquals((1, 0),
perpVector)
def test_negativeX_negativeY(self):
startVector = (-1, 0)
perpVector = vector.getRightPerpendicular(startVector)
self.assertEquals((0, 1),
perpVector)
def test_positiveX_positiveY(self):
startVector = (1, 0)
perpVector = vector.getRightPerpendicular(startVector)
self.assertEquals((0, -1),
perpVector)
