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 myNextCollisionPoint(self):
if not self.nextCollider:
self.myNextCollider()
if not self.nextCollider:
vec.set(Inf, self.cp)
return self.cp
# TODO: This was already calculated in myNextCollider
return self.cp
def myNextCollisionPoint(self):
if not self.nextCollider:
self.myNextCollider()
if not self.nextCollider:
vec.set(Inf, self.cp)
return self.cp
# TODO: This was already calculated in myNextCollider
return self.cp