def flock(agent, neighbors, separationMagnitude=50, alignmentMagnitude=2, cohesionMagnitude=1): separationForce = separate.separate(agent, neighbors) alignmentForce = align.align(agent, neighbors) cohesionForce = cohere.cohere(agent, neighbors) weightedSeparationForce = calculate.multiplyVectorAndScalar(separationForce, separationMagnitude) weightedAlignmentForce = calculate.multiplyVectorAndScalar(alignmentForce, alignmentMagnitude) weightedCohesionForce = calculate.multiplyVectorAndScalar(cohesionForce, cohesionMagnitude) totalForce = calculate.addVectors(weightedSeparationForce, weightedAlignmentForce, weightedCohesionForce) return totalForce
def test_scalarIsZero_vectorIsZero(self): startVector = (1, 1) newVector =\ calculate.multiplyVectorAndScalar(vector=startVector, scalar=0) self.assertEquals((0,0), newVector)
def cohere(agent, neighbors): if not neighbors: return (0, 0) agentPosition = agent.getPosition() cumulativeVector = (0, 0) neighborCount = 0 for neighbor in neighbors: if neighbor == agent: continue neighborCount += 1 originToNeighbor = neighbor.getPosition() cumulativeVector = calculate.addVectors(cumulativeVector, originToNeighbor) if not neighborCount: return (0, 0) originToCenterPoint = calculate.multiplyVectorAndScalar( cumulativeVector, float(1) / neighborCount) centerPoint = originToCenterPoint return seek.seek(agent, centerPoint)
def test_scalarIsTwo_vectorIsDoubled(self): startVector = (2, 2) newVector =\ calculate.multiplyVectorAndScalar(vector=startVector, scalar=2) self.assertEquals((4, 4), newVector)
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 prioritizedRunningSum(self): force = (0, 0) remaining_reservoir = self.force_reservoir for behaviorKeyword in self.keywords: action = self.actions[behaviorKeyword] if action is None: continue arguments = (self.agent,) + action behaviorFunction = self.steeringFunctions[behaviorKeyword] weight = self.weight[behaviorKeyword] forceForBehavior = calculate.multiplyVectorAndScalar(behaviorFunction(*arguments), weight) forceForBehavior = vector.truncate(forceForBehavior, remaining_reservoir) magnitudeForBehaviorForce = vector.getMagnitude(forceForBehavior) remaining_reservoir -= magnitudeForBehaviorForce force = calculate.addVectors(force, forceForBehavior) if remaining_reservoir <= 0: break force = vector.truncate(force, self.agent.getMaxForce()) return force
def test_scalarIsOne_vectorIsUnchanged(self): startVector = (2, 2) newVector =\ calculate.multiplyVectorAndScalar(vector=startVector, scalar=1) self.assertEquals((2, 2), newVector)
def pursueoffset(agent, targetAgent, targetToOffset): agentPosition = agent.getPosition() agentMaxSpeed = agent.getMaxSpeed() targetPosition = targetAgent.getPosition() targetDirection = targetAgent.getDirection() targetSpeed = targetAgent.getSpeed() targetVelocity = targetAgent.getVelocity() worldTargetToOffset = convert.vectorToWorldSpace(targetToOffset, targetPosition, targetDirection) offsetPosition = calculate.addPointAndVector(targetPosition, worldTargetToOffset) agentToOffset = calculate.subtractPoints(offsetPosition, agentPosition) distanceToOffset = vector.getMagnitude(agentToOffset) if targetSpeed == 0: lookAheadTime = 0 else: lookAheadTime = distanceToOffset / (agentMaxSpeed + targetSpeed) targetToPredictedPosition = calculate.multiplyVectorAndScalar(targetVelocity, lookAheadTime) predictedOffsetPosition = calculate.addPointAndVector(offsetPosition, targetToPredictedPosition) return arrive.arrive(agent, predictedOffsetPosition, .9)
def separate(agent, neighbors): agentPosition = agent.getPosition() cumulativeForce = (0, 0) for neighbor in neighbors: if neighbor == agent: continue neighborPosition = neighbor.getPosition() neighborToAgent = calculate.subtractPoints(agentPosition, neighborPosition) distanceToAgent = vector.getMagnitude(neighborToAgent) if distanceToAgent == 0: neighborHeadingToAgent = vector.normalize((random.random() - 1, random.random() - 1)) magnitude = 100 else: neighborHeadingToAgent = vector.normalize(neighborToAgent) magnitude = max(agent.length, agent.width) / distanceToAgent separationForceForThisNeighbor =\ calculate.multiplyVectorAndScalar(neighborHeadingToAgent, magnitude) cumulativeForce = calculate.addVectors(cumulativeForce, separationForceForThisNeighbor) return cumulativeForce
def pursue(agent, target): agentHeading = agent.getHeading() targetHeading = target.getHeading() targetPosition = target.getPosition() relativeHeading = calculate.dotProduct(agentHeading, targetHeading) #If the target is heading at me, then just Seek if relativeHeading < -.95: return seek.seek(agent, targetPosition) agentPosition = agent.getPosition() agentMaxSpeed = agent.getMaxSpeed() targetSpeed = target.getSpeed() targetVelocity = target.getVelocity() agentToTarget = calculate.subtractPoints(targetPosition, agentPosition) distanceToTarget = vector.getMagnitude(agentToTarget) lookAheadTime = distanceToTarget / (agentMaxSpeed + targetSpeed) lookAheadVector = calculate.multiplyVectorAndScalar(targetVelocity, lookAheadTime) lookAheadPosition = calculate.addPointAndVector(targetPosition, lookAheadVector) return seek.seek(agent, lookAheadPosition)
def test_scalarIsOneHalf_vectorIsHalved(self): startVector = (2, 2) newVector =\ calculate.multiplyVectorAndScalar(vector=startVector, scalar=.5) self.assertEquals((1, 1), newVector)
def plotLocalRouteAroundTarget(owner, target): steeringController = owner.steeringController ownerPosition = owner.getPosition() targetPosition = target.getPosition() ownerToTarget = calculate.subtractPoints(targetPosition, ownerPosition) distanceToTarget = vector.getMagnitude(ownerToTarget) optimalPathDistance = min(distanceToTarget, owner.getMaximumFiringRange()) #These are the eight "compass" directions, projected #in the target's local space vectors = ((0, 1), (1, 1), (1, 0), (-1, 1), (-1, 0), (-1, -1), (0, -1), (1, -1)) #Now scale the directions so that they have my magnitude. #We can treat these vectors as points in an octagonal #path around the target - a path scaled to an optimal distance. pathPoints = [calculate.multiplyVectorAndScalar(normalizedVector, optimalPathDistance) for normalizedVector in vectors] #Find the point in the path that is closest to my position. ownerLocalPosition = convert.pointToLocalSpace(ownerPosition, targetPosition, target.getDirection()) closestDistanceSquared = None closestIndex = None for index in range(len(pathPoints)): pathPoint = pathPoints[index] ownerToPathPoint = calculate.subtractPoints(pathPoint, ownerLocalPosition) distanceSquaredToPathPoint =\ vector.getMagnitudeSquared(ownerToPathPoint) if (closestDistanceSquared is None or distanceSquaredToPathPoint < closestDistanceSquared): closestIndex = index closestDistanceSquared = distanceSquaredToPathPoint #Now "shift" the path so that my closest point is the first in the list. path = pathPoints[closestIndex:] + pathPoints[:closestIndex] #Plot a course to visit the path. If at any point I find a clear shot to the target, #I will dive at it. steeringController.plotPath(path, closed=True)
def interpose(agent, enemy, charge): agentPosition = agent.getPosition() agentMaxSpeed = agent.getMaxSpeed() enemyPosition = enemy.getPosition() enemyVelocity = enemy.getVelocity() chargePosition = charge.getPosition() chargeVelocity = charge.getVelocity() enemyToCharge = calculate.subtractPoints(chargePosition, enemyPosition) midVector = calculate.multiplyVectorAndScalar(enemyToCharge, .5) midPoint = calculate.addPointAndVector(enemyPosition, midVector) agentToMidPoint = calculate.subtractPoints(midPoint, agentPosition) distanceToMidPoint = vector.getMagnitude(agentToMidPoint) timeToMidPoint = distanceToMidPoint / agentMaxSpeed enemyToFuturePosition = calculate.multiplyVectorAndScalar(enemyVelocity, timeToMidPoint) enemyFuturePosition = calculate.addPointAndVector(enemyPosition, enemyToFuturePosition) chargeToFuturePosition = calculate.multiplyVectorAndScalar(chargeVelocity, timeToMidPoint) chargeFuturePosition = calculate.addPointAndVector(chargePosition, chargeToFuturePosition) enemyFutureToChargeFuture = calculate.subtractPoints(chargeFuturePosition, enemyFuturePosition) futureMidVector = calculate.multiplyVectorAndScalar(enemyFutureToChargeFuture, .5) futureMidPoint = calculate.addPointAndVector(enemyFuturePosition, futureMidVector) return arrive.arrive(agent, futureMidPoint)
def weightedTruncatedSum(self): force = (0, 0) action_list = self.action_list for behavior in self.keyword_list: if self.on(behavior): forceForBehavior = calculate.multiplyVectorAndScalar(action_list[behavior].getWeight(), action_list[behavior].executeFunction()) force = calculate.addVectors(force, forceForBehavior) force = vector.truncate(force, self.parent_agent.getMaxForce()) return force
def align(agent, neighbors): #Save us from divide-by-zero if not neighbors: return (0, 0) cumulativeHeading = (0, 0) for neighbor in neighbors: neighborVelocity = neighbor.getVelocity() neighborHeading = vector.normalize(neighborVelocity) cumulativeHeading = calculate.addVectors(cumulativeHeading, neighborHeading) averageHeading = calculate.multiplyVectorAndScalar(cumulativeHeading, float(1) / len(neighbors)) return averageHeading
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 predictFuturePosition(self, source, target, shotSpeed): sourcePosition = source.getPosition() targetPosition = target.getPosition() targetVelocity = target.getVelocity() targetSpeed = target.getSpeed() sourceToTarget = calculate.subtractPoints(targetPosition, sourcePosition) manhattanDistanceToTarget = vector.getManhattanMagnitude(sourceToTarget) lookAheadTime = manhattanDistanceToTarget / (shotSpeed + targetSpeed) lookAheadVector = calculate.multiplyVectorAndScalar(targetVelocity, lookAheadTime) lookAheadPosition = calculate.addPointAndVector(targetPosition, lookAheadVector) return lookAheadPosition
def update(self, timeElapsed): current_time = self.world.current_time if self.timeOfNormalColor and current_time < self.timeOfNormalColor: self.color = (random.random(), random.random(), random.random()) elif self.timeOfNormalColor: self.timeOfNormalColor = None self.color = self.normalColor world = self.world maxspeed = self.maxSpeed throttlespeed = self.throttleSpeed maxforce = self.maxForce minDetectionLength = self.minDetectionLength for stateMachine in self.stateMachines: stateMachine.update() for turret in self.turrets: turret.update(timeElapsed) self.launch() force = self.steeringController.calculate() force = vector.truncate(vectorTuple=force, cap=maxforce) acceleration = calculate.multiplyVectorAndScalar(vector=force, scalar=timeElapsed / (self.mass * 1000.0)) velocity = calculate.addVectors(self.velocity, acceleration) velocity = vector.truncate(velocity, throttlespeed) self.velocity = velocity speed = vector.getMagnitude(velocity) (x, y) = calculate.addPointAndVector(self.position, velocity) self.position = (x, y) self.obstacleDetectionDimensions[0] =\ minDetectionLength + (speed / maxspeed) * minDetectionLength
def evade(agent, target, evadeDistanceSquared=None): agentPosition = agent.getPosition() agentMaxSpeed = agent.getMaxSpeed() targetPosition = target.getPosition() targetSpeed = target.getSpeed() targetVelocity = target.getVelocity() targetToAgent = calculate.subtractPoints(agentPosition, targetPosition) distanceToTarget = vector.getMagnitude(targetToAgent) lookAheadTime = distanceToTarget / (agentMaxSpeed + targetSpeed) lookAheadVector = calculate.multiplyVectorAndScalar(targetVelocity, lookAheadTime) lookAheadPosition = calculate.addPointAndVector(targetPosition, lookAheadVector) return flee.flee(agent, lookAheadPosition, evadeDistanceSquared)
def arrive(agent, targetPosition, decelerationFactor=None): steeringController = agent.getSteeringController() if decelerationFactor is None: decelerationFactor = steeringController.decelerationFactor agentPosition = agent.getPosition() agentToTarget = calculate.subtractPoints(targetPosition, agentPosition) distanceToTarget = vector.getMagnitude(agentToTarget) if round(distanceToTarget) > 0: agentMaxSpeed = agent.getMaxSpeed() agentVelocity = agent.getVelocity() speed = distanceToTarget / decelerationFactor speed = min(speed, agentMaxSpeed) desiredVelocity = calculate.multiplyVectorAndScalar(agentToTarget, speed / distanceToTarget) return calculate.subtractVectors(desiredVelocity, agentVelocity) else: return (0, 0)