def distance_2p(p1, p2):
"""Returns the euclidean distance between two points
arg keywords:
p1 - a vector
p2 - a vector
returns: a number
"""
return norm(p2 - p1)
def make_hcs_2d_scaled(a, b):
"""build a 2D homogeneus coordiate system from two vectors, but scale with distance between input point"""
u = b - a
if tol_eq(norm(u), 0.0): # 2006/6/30
return None
#else:
# u = u / norm(u)
v = vector([-u[1], u[0]])
hcs = matrix_factory([[u[0], v[0], a[0]], [u[1], v[1], a[1]],
[0.0, 0.0, 1.0]])
return hcs
def test_rr_int():
"""test random ray-ray intersection. returns True iff succesful"""
# generate tree points A,B,C an two rays AC, BC.
# then calculate the intersection of the two rays
# and check that it equals C
p_a = randvec(2, 0.0, 10.0, 1.0)
p_b = randvec(2, 0.0, 10.0, 1.0)
p_c = randvec(2, 0.0, 10.0, 1.0)
# print p_a, p_b, p_c
if tol_eq(norm(p_c - p_a), 0) or tol_eq(norm(p_c - p_b), 0):
return True # ignore this case
v_ac = (p_c - p_a) / norm(p_c - p_a)
v_bc = (p_c - p_b) / norm(p_c - p_b)
s = rr_int(p_a, v_ac, p_b, v_bc)
if tol_eq(math.fabs(dot(v_ac, v_bc)), 1.0):
return len(s) == 0
else:
if len(s) > 0:
p_s = s[0]
return tol_eq(p_s[0], p_c[0]) and tol_eq(p_s[1], p_c[1])
else:
return False
def _merge_transform_2D(self, other):
"""returns a new configurations which is this one plus the given other configuration transformed, such that common points will overlap (if possible)."""
shared = Set(self.vars()).intersection(other.vars())
underconstrained = self.underconstrained or other.underconstrained
if len(shared) == 0:
underconstrained = True
cs1 = make_hcs_2d(vector([0.0, 0.0]), vector([1.0, 0.0]))
cs2 = make_hcs_2d(vector([0.0, 0.0]), vector([1.0, 0.0]))
elif len(shared) == 1:
if len(self.vars()) > 1 and len(other.vars()) > 1:
underconstrained = True
v1 = list(shared)[0]
p11 = self.map[v1]
p21 = other.map[v1]
cs1 = make_hcs_2d(p11, p11 + vector([1.0, 0.0]))
cs2 = make_hcs_2d(p21, p21 + vector([1.0, 0.0]))
else: # len(shared) >= 2:
v1 = list(shared)[0]
v2 = list(shared)[1]
p11 = self.map[v1]
p12 = self.map[v2]
if tol_eq(norm(p12 - p11), 0.0):
underconstrained = True
cs1 = make_hcs_2d(p11, p11 + vector[1.0, 0.0])
else:
cs1 = make_hcs_2d(p11, p12)
p21 = other.map[v1]
p22 = other.map[v2]
if tol_eq(norm(p22 - p21), 0.0):
underconstrained = True
cs2 = make_hcs_2d(p21, p21 + vector[1.0, 0.0])
else:
cs2 = make_hcs_2d(p21, p22)
# in any case
t = cs_transform_matrix(cs2, cs1)
t.underconstrained = underconstrained
return t
def _merge_scale_transform_3D(self, other):
shared = set(self.vars()).intersection(other.vars())
if len(shared) == 0:
return self._merge_transform_3D(other)
elif len(shared) == 1:
return self._merge_transform_3D(other)
elif len(shared) >= 2:
v1 = list(shared)[0]
p1s = self.map[v1]
p1o = other.map[v1]
v2 = list(shared)[1]
p2s = self.map[v2]
p2o = other.map[v2]
scale = norm(p2s - p1s) / norm(p2o - p1o)
scale_trans = pivot_scale_3D(p1o, scale)
diag_print("scale_trans = " + str(scale_trans),
"Configuration.merge_scale_transform_3D")
merge_trans = self._merge_transform_3D(other)
diag_print("merge_trans = " + str(merge_trans),
"Configuration.merge_scale_transform_3D")
# merge_scale_trans = scale_trans.mmul(merge_trans)
merge_scale_trans = merge_trans.mmul(scale_trans)
merge_scale_trans.underconstrained = merge_trans.underconstrained
return merge_scale_trans
def make_hcs_3d_scaled(a, b, c):
"""build a 3D homogeneus coordiate system from three vectors"""
# create orthnormal basis
u = b - a
u = u / norm(u)
v = c - a
v = v / norm(v)
w = cross(u, v)
v = cross(w, u)
# scale
u = u / norm(u) / norm(b - a)
v = v / norm(v) / norm(c - a)
hcs = matrix_factory([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def cc_int(p1, r1, p2, r2):
"""
Intersect circle (p1,r1) circle (p2,r2)
where p1 and p2 are 2-vectors and r1 and r2 are scalars
Returns a list of zero, one or two solution points.
"""
d = norm(p2 - p1)
if not tol_gt(d, 0):
return []
u = ((r1 * r1 - r2 * r2) / d + d) / 2
if tol_lt(r1 * r1, u * u):
return []
elif r1 * r1 < u * u:
v = 0.0
else:
v = math.sqrt(r1 * r1 - u * u)
s = (p2 - p1) * u / d
if tol_eq(norm(s), 0):
p3a = p1 + vector([p2[1] - p1[1], p1[0] - p2[0]]) * r1 / d
if tol_eq(r1 / d, 0):
return [p3a]
else:
p3b = p1 + vector([p1[1] - p2[1], p2[0] - p1[0]]) * r1 / d
return [p3a, p3b]
else:
print("***")
print(p1)
print(s)
print(vector([s[1], -s[0]]))
print(v)
print(norm(s))
print("***")
p3a = p1 + s + vector([s[1], -s[0]]) * v / norm(s)
if tol_eq(v / norm(s), 0):
return [p3a]
else:
p3b = p1 + s + vector([-s[1], s[0]]) * v / norm(s)
return [p3a, p3b]
def test_sss_int():
p1 = randvec(3, 0.0, 10.0, 1.0)
p2 = randvec(3, 0.0, 10.0, 1.0)
p3 = randvec(3, 0.0, 10.0, 1.0)
p4 = randvec(3, 0.0, 10.0, 1.0)
#p1 = vector([0.0,0.0,0.0])
#p2 = vector([1.0,0.0,0.0])
#p3 = vector([0.0,1.0,0.0])
#p4 = vector([1.0,1.0,1.0])
d14 = norm(p4 - p1)
d24 = norm(p4 - p2)
d34 = norm(p4 - p3)
sols = sss_int(p1, d14, p2, d24, p3, d34)
sat = True
for sol in sols:
# print sol
d1s = norm(sol - p1)
d2s = norm(sol - p2)
d3s = norm(sol - p3)
sat = sat and tol_eq(d1s, d14)
sat = sat and tol_eq(d2s, d24)
sat = sat and tol_eq(d3s, d34)
# print sat
return sat
def sss_int(p1, r1, p2, r2, p3, r3):
"""Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3
respectively.
Returns a list of zero, one or two solution points.
"""
solutions = []
# plane though p1, p2, p3
n = cross(p2 - p1, p3 - p1)
n = n / norm(n)
# intersect circles in plane
cp1 = vector([0.0, 0.0])
cp2 = vector([norm(p2 - p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# px, rx, nx is circle
px = p1 + (p2 - p1) * cpxs[0][0] / norm(p2 - p1)
rx = abs(cpxs[0][1])
# plane of intersection cicle
nx = p2 - p1
nx = nx / norm(nx)
# print "px,rx,nx:",px,rx,nx
# py = project p3 on px,nx
dy3 = dot(p3 - px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
ry = math.sin(math.acos(abs(dy3 / r3))) * r3
# print "py,ry:",py,ry
cpx = vector([0.0, 0.0])
cpy = vector([norm(py - px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py - px) * cp4[0] / norm(py - px) + n * cp4[1]
solutions.append(p4)
return solutions
def _merge_transform_3D(self, other):
"""returns a matrix for a rigid transformation
such that points in other are mapped onto points in self
"""
shared = set(self.vars()).intersection(other.vars())
underconstrained = self.underconstrained or other.underconstrained
if len(shared) == 0:
underconstrained = True
cs1 = make_hcs_3d(vector([0.0, 0.0, 0.0]), vector([0.0, 1.0, 0.0]),
vector([0.0, 0.0, 1.0]))
cs2 = make_hcs_3d(vector([0.0, 0.0, 0.0]), vector([0.0, 1.0, 0.0]),
vector([0.0, 0.0, 1.0]))
elif len(shared) == 1:
if len(self.vars()) > 1 and len(other.vars()) > 1:
underconstrained = True
v1 = list(shared)[0]
p1s = self.map[v1]
p1o = other.map[v1]
cs1 = make_hcs_3d(p1s, p1s + vector([1.0, 0.0, 0.0]),
p1s + vector([0.0, 1.0, 0.0]))
cs2 = make_hcs_3d(p1o, p1o + vector([1.0, 0.0, 0.0]),
p1o + vector([0.0, 1.0, 0.0]))
elif len(shared) == 2:
if len(self.vars()) > 2 and len(other.vars()) > 2:
underconstrained = True
v1 = list(shared)[0]
p1s = self.map[v1]
p1o = other.map[v1]
v2 = list(shared)[1]
p2s = self.map[v2]
p2o = other.map[v2]
p3s = p1s + cross(p2s - p1s, perp2D(p2s - p1s))
p3o = p1o + cross(p2o - p1o, perp2D(p2s - p1s))
if tol_eq(norm(p2s - p1s), 0.0):
underconstrained = True
cs1 = make_hcs_3d(p1s, p2s, p3s)
cs2 = make_hcs_3d(p1o, p2o, p3o)
else: # len(shared) >= 3:
v1 = list(shared)[0]
v2 = list(shared)[1]
v3 = list(shared)[2]
p1s = self.map[v1]
p2s = self.map[v2]
p3s = self.map[v3]
cs1 = make_hcs_3d(p1s, p2s, p3s)
if tol_eq(norm(p2s - p1s), 0.0):
underconstrained = True
if tol_eq(norm(p3s - p1s), 0.0):
underconstrained = True
if tol_eq(norm(p3s - p2s), 0.0):
underconstrained = True
p1o = other.map[v1]
p2o = other.map[v2]
p3o = other.map[v3]
cs2 = make_hcs_3d(p1o, p2o, p3o)
if tol_eq(norm(p2o - p1o), 0.0):
underconstrained = True
if tol_eq(norm(p3o - p1o), 0.0):
underconstrained = True
if tol_eq(norm(p3o - p2o), 0.0):
underconstrained = True
# in any case:
t = cs_transform_matrix(cs2, cs1)
t.underconstrained = underconstrained
return t