Newer
Older
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
# 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)