Skip to content
Snippets Groups Projects
triquad.py 37.1 KiB
Newer Older
  • Learn to ignore specific revisions
  • 
    def Norm3(x, y, z):
        """Return vector (x,y,z) normalized by dividing by squared length.
        Return (0.0, 0.0, 1.0) if the result is undefined."""
        sqrlen = x * x + y * y + z * z
        if sqrlen < 1e-100:
            return (0.0, 0.0, 1.0)
        else:
            try:
                d = sqrt(sqrlen)
                return (x / d, y / d, z / d)
            except:
                return (0.0, 0.0, 1.0)
    
    
    # We're using right-hand coord system, where
    # forefinger=x, middle=y, thumb=z on right hand.
    # Then, e.g., (1,0,0) x (0,1,0) = (0,0,1)
    def Cross3(a, b):
        """Return the cross product of two vectors, a x b."""
    
        (ax, ay, az) = a
        (bx, by, bz) = b
        return (ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx)
    
    
    def Dot2(a, b):
        """Return the dot product of two 2d vectors, a . b."""
    
        return a[0] * b[0] + a[1] * b[1]
    
    
    def Perp2(a, b):
        """Return a sort of 2d cross product."""
    
        return a[0] * b[1] - a[1] * b[0]
    
    
    def Sub2(a, b):
        """Return difference of 2d vectors, a-b."""
    
        return (a[0] - b[0], a[1] - b[1])
    
    
    def Add2(a, b):
        """Return the sum of 2d vectors, a+b."""
    
        return (a[0] + b[0], a[1] + b[1])
    
    
    def Length2(v):
        """Return length of vector v=(x,y)."""
    
        return sqrt(v[0] * v[0] + v[1] * v[1])
    
    
    def LinInterp2(a, b, alpha):
        """Return the point alpha of the way from a to b."""
    
        beta = 1 - alpha
        return (beta * a[0] + alpha * b[0], beta * a[1] + alpha * b[1])
    
    
    def Normalized2(p):
        """Return vector p normlized by dividing by its squared length.
        Return (0.0, 1.0) if the result is undefined."""
    
        (x, y) = p
        sqrlen = x * x + y * y
        if sqrlen < 1e-100:
            return (0.0, 1.0)
        else:
            try:
                d = sqrt(sqrlen)
                return (x / d, y / d)
            except:
                return (0.0, 1.0)
    
    
    def Angle(a, b, c, points):
        """Return Angle abc in degrees, in range [0,180),
        where a,b,c are indices into points."""
    
        u = Sub2(points.pos[c], points.pos[b])
        v = Sub2(points.pos[a], points.pos[b])
        n1 = Length2(u)
        n2 = Length2(v)
        if n1 == 0.0 or n2 == 0.0:
            return 0.0
        else:
            costheta = Dot2(u, v) / (n1 * n2)
            if costheta > 1.0:
                costheta = 1.0
            if costheta < - 1.0:
                costheta = - 1.0
            return math.acos(costheta) * 180.0 / math.pi
    
    
    def SegsIntersect(ixa, ixb, ixc, ixd, points):
        """Return true if segment AB intersects CD,
        false if they just touch.  ixa, ixb, ixc, ixd are indices
        into points."""
    
        a = points.pos[ixa]
        b = points.pos[ixb]
        c = points.pos[ixc]
        d = points.pos[ixd]
        u = Sub2(b, a)
        v = Sub2(d, c)
        w = Sub2(a, c)
        pp = Perp2(u, v)
        if abs(pp) > TOL:
            si = Perp2(v, w) / pp
            ti = Perp2(u, w) / pp
            return 0.0 < si < 1.0 and 0.0 < ti < 1.0
        else:
            # parallel or overlapping
            if Dot2(u, u) == 0.0 or Dot2(v, v) == 0.0:
                return False
            else:
                pp2 = Perp2(w, v)
                if abs(pp2) > TOL:
                    return False  # parallel, not collinear
                z = Sub2(b, c)
                (vx, vy) = v
                (wx, wy) = w
                (zx, zy) = z
                if vx == 0.0:
                    (t0, t1) = (wy / vy, zy / vy)
                else:
                    (t0, t1) = (wx / vx, zx / vx)
                return 0.0 < t0 < 1.0 and 0.0 < t1 < 1.0
    
    
    def Ccw(a, b, c, points):
        """Return true if ABC is a counterclockwise-oriented triangle,
        where a, b, and c are indices into points.
        Returns false if not, or if colinear within TOL."""
    
        (ax, ay) = (points.pos[a][0], points.pos[a][1])
        (bx, by) = (points.pos[b][0], points.pos[b][1])
        (cx, cy) = (points.pos[c][0], points.pos[c][1])
        d = ax * by - bx * ay - ax * cy + cx * ay + bx * cy - cx * by
        return d > TOL
    
    
    def InCircle(a, b, c, d, points):
        """Return true if circle through points with indices a, b, c
        contains point with index d (indices into points).
        Except: if ABC forms a counterclockwise oriented triangle
        then the test is reversed: return true if d is outside the circle.
        Will get false, no matter what orientation, if d is cocircular, with TOL^2.
          | xa ya xa^2+ya^2 1 |
          | xb yb xb^2+yb^2 1 | > 0
          | xc yc xc^2+yc^2 1 |
          | xd yd xd^2+yd^2 1 |
        """
    
        (xa, ya, za) = _Icc(points.pos[a])
        (xb, yb, zb) = _Icc(points.pos[b])
        (xc, yc, zc) = _Icc(points.pos[c])
        (xd, yd, zd) = _Icc(points.pos[d])
        det = xa * (yb * zc - yc * zb - yb * zd + yd * zb + yc * zd - yd * zc) \
              - xb * (ya * zc - yc * za - ya * zd + yd * za + yc * zd - yd * zc) \
              + xc * (ya * zb - yb * za - ya * zd + yd * za + yb * zd - yd * zb) \
              - xd * (ya * zb - yb * za - ya * zc + yc * za + yb * zc - yc * zb)
        return det > TOL * TOL
    
    
    def _Icc(p):
        (x, y) = (p[0], p[1])
        return (x, y, x * x + y * y)