def nearestIntersection(c0, c1, q):
p = c1.position
v = c1.orientation
rp = p - c0.position
k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
k1 = vec.dot(v, rp)
roots = []
k = k1 * k1 - k0
if util.isAlmostZero(k):
roots.append(-k1)
elif 0.0 < k:
kSqrt = math.sqrt(k)
roots.append(-k1 - kSqrt)
roots.append(-k1 + kSqrt)
assert roots[0] < roots[1]
vec.set(Inf, q)
for root in roots:
if util.isAlmostZero(root) or 0 < root:
q = vec.scale(v, root, q)
q = q + p
break
return q
def nearestIntersection(c0, c1, q):
p = c1.position
v = c1.orientation
rp = p - c0.position
k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
k1 = vec.dot(v, rp)
roots = []
k = k1 * k1 - k0
if util.isAlmostZero(k):
roots.append(-k1)
elif 0.0 < k:
kSqrt = math.sqrt(k)
roots.append(-k1 - kSqrt)
roots.append(-k1 + kSqrt)
assert roots[0] < roots[1]
vec.set(Inf, q)
for root in roots:
if util.isAlmostZero(root) or 0 < root:
q = vec.scale(v, root, q)
q = q + p
break
return q
def normalize(u, v):
l = length(u)
if (util.isAlmostZero(l)):
zeroize(v)
else:
scale(u, 1.0/l, v)
return v
def setVelocity(self, velocity):
s = vec.length(velocity)
if util.isAlmostZero(s):
self.setSpeed(0)
# Don't change the orientation if the speed is zero.
else:
self.setSpeed(s)
self.shape.setOrientation(vec.normalize(velocity, self.velocity))
def normalize(u, v):
l = length(u)
if (util.isAlmostZero(l)):
zeroize(v)
else:
scale(u, 1.0 / l, v)
return v
def setVelocity(self, velocity):
s = vec.length(velocity)
if util.isAlmostZero(s):
self.setSpeed(0)
# Don't change the orientation if the speed is zero.
else:
self.setSpeed(s)
self.shape.setOrientation(vec.normalize(velocity, self.velocity))
def angle(u):
l = vec.length(u)
if util.isAlmostZero(l):
return 0.0
assert util.isAlmostEq(vec.length(u), 1.0), "input must be normalized, not " + str(u)
return math.atan2(u[1], u[0])
def angle(u):
l = vec.length(u)
if util.isAlmostZero(l):
return 0.0
assert util.isAlmostEq(vec.length(u),
1.0), "input must be normalized, not " + str(u)
return math.atan2(u[1], u[0])
def clampMaxLength(u, x, v):
l = length(u)
if l < x:
copy(u, v)
elif util.isAlmostZero(l):
zeroize(v)
else:
scale(u, x/l, v)
return v
def clampMaxLength(u, x, v):
l = length(u)
if l < x:
copy(u, v)
elif util.isAlmostZero(l):
zeroize(v)
else:
scale(u, x / l, v)
return v
def calcAction(self):
# TODO: make this settable
soonThreshold = 50.0
# TODO: consider re-factoring using BrainConditional (or something like it)
# TODO: this is hardwired to only avoid static obstacles
if soonThreshold < self.percepts.myTimeToCollision() or (self.percepts.myNextCollider() and Inf != self.percepts.nextCollider.mass):
# No collision danger
time = self.percepts.getTime()
# How many milliseconds to wait after a potential collision was detected before
# resuming with the default controller. TODO: consider making a settable class variable.
delay = 0.5
if self.timeLastCollisionDetected < 0 or delay < time - self.timeLastCollisionDetected:
self.defaultBrain.calcAction()
self.action = self.defaultBrain.action
else:
# Just continue with last action.
# TODO: consider some time discounted blend of default controller and
# avoidance vector.
pass
return
self.timeLastCollisionDetected = self.percepts.getTime()
# Collision danger present so need to take evasive action.
self.rp = vec.normalize(self.percepts.myNextCollisionPoint() - self.percepts.myPosition(), self.rp)
self.tmp = util2D.perpendicularTo(self.rp, self.percepts.myNextCollider().normalTo(self.percepts.getMe(), self.tmp), self.tmp)[0]
assert util.isAlmostZero(vec.dot(self.rp, self.tmp))
self.action.setDirection(self.tmp)
# TODO: modulate the speed based on time until collision and whatever the defaultControler
# set it to.
self.action.setSpeed(1.0)
def calcAction(self):
# TODO: make this settable
soonThreshold = 50.0
# TODO: consider re-factoring using BrainConditional (or something like it)
# TODO: this is hardwired to only avoid static obstacles
if soonThreshold < self.percepts.myTimeToCollision() or (self.percepts.myNextCollider() and Inf != self.percepts.nextCollider.mass):
# No collision danger
time = self.percepts.getTime()
# How many milliseconds to wait after a potential collision was detected before
# resuming with the default controller. TODO: consider making a settable class variable.
delay = 0.5
if self.timeLastCollisionDetected < 0 or delay < time - self.timeLastCollisionDetected:
self.defaultBrain.calcAction()
self.action = self.defaultBrain.action
else:
# Just continue with last action.
# TODO: consider some time discounted blend of default controller and
# avoidance vector.
pass
return
self.timeLastCollisionDetected = self.percepts.getTime()
# Collision danger present so need to take evasive action.
self.rp = vec.normalize(self.percepts.myNextCollisionPoint() - self.percepts.myPosition(), self.rp)
self.tmp = util2D.perpendicularTo(self.rp, self.percepts.myNextCollider().normalTo(self.percepts.getMe(), self.tmp), self.tmp)[0]
assert util.isAlmostZero(vec.dot(self.rp, self.tmp))
self.action.setDirection(self.tmp)
# TODO: modulate the speed based on time until collision and whatever the defaultControler
# set it to.
self.action.setSpeed(1.0)
def isAlmostZero(u):
for x in u:
if not util.isAlmostZero(x):
return False
return True
def isAlmostZero(u):
for x in u:
if not util.isAlmostZero(x):
return False
return True
