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
示例#2
0
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
示例#3
0
    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