def make_hcs_3d_scaled (a, b, c):
"""build a 3D homogeneus coordiate system from three points, and derive scale from distance between points"""
# create orthonormal basis
u = normalised(b-a)
v = normalised(c-a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu,0) and tol_eq(nv,0):
# all points equal, no rotation
u = vector.vector([1.0,0.0,0.0])
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0):
# determine u perpendicular from v
u,dummy = perp_3d(v)[0]
elif tol_eq(nv, 0):
# determine v perpendicular from u
dummy,v = perp_3d(u)[0]
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
# scale again
if not tol_eq(vector.norm(b-a),0.0):
u = u / vector.norm(b-a)
if not tol_eq(vector.norm(c-a),0.0):
v = v / vector.norm(c-a)
# note: w is not scaled
# create matix with basis vectors + translation as columns
hcs = Mat([
[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 make_hcs_3d (a, b, c, righthanded=True):
"""build a 3D homogeneous coordiate system from three points. The origin is point a. The x-axis is
b-a, the y axis is c-a, or as close as possible after orthogonormalisation."""
# create orthonormal basis
u = normalised(b-a)
v = normalised(c-a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu,0.0) and tol_eq(nv,0.0):
# all points equal, no rotation
u = vector.vector([1.0,0.0,0.0])
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0.0):
# determine u perpendicular from v
u,dummy = perp_3d(v)
elif tol_eq(nv, 0.0):
# determine v perpendicular from u
dummy,v = perp_3d(u)
# ensure that u and v are different
if tol_eq(vector.norm(u-v),0.0):
dummy,v = perp_3d(u)
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
# flip basis if lefthanded desired
if righthanded==False:
w = -w
# create matix with basis vectors + translation as columns
hcs = Mat([
[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 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 = vector.cross(p2 - p1, p3 - p1)
n = n / vector.norm(n)
# intersect circles in plane
cp1 = vector.vector([0.0, 0.0])
cp2 = vector.vector([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] / vector.norm(p2 - p1)
rx = abs(cpxs[0][1])
# plane of intersection cicle
nx = p2 - p1
nx = nx / vector.norm(nx)
# print "px,rx,nx:",px,rx,nx
# py = project p3 on px,nx
dy3 = vector.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.vector([0.0, 0.0])
cpy = vector.vector([vector.norm(py - px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py - px) * cp4[0] / vector.norm(py - px) + n * cp4[1]
solutions.append(p4)
return solutions
def step(self, myOrder, dT=0.01):
# initialize variables:
tanks = self.ship.get_tanks()
desired_vel_c, desired_ang_vel_c = myOrder.getorder()
myShip = self.ship
acc = []
ang_acc = []
for engine, orientation in myShip.engines:
f = orientation * engine.thrust
r = Vector(0, 0, myShip.get_mod_pos(engine))
tq = cross(r, f)
a = f / myShip.mass
tqx, tqy, tqz = tq.getval()
moixy, moiz = self.moi
alpha = Vector(tqx / moixy, tqy / moixy, tqz / moiz)
acc.append(a)
ang_acc.append(alpha)
vel_pblm = vec_to_pbl_matrix(desired_vel_c, acc)
ang_pblm = vec_to_pbl_matrix(desired_ang_vel_c, ang_acc)
pblmtx = Matrix(
len(vel_pblm.mat) + len(ang_pblm.mat),
len(myShip.engines) + 1)
pblmtx.mat = vel_pblm.mat
pblmtx.mat.extend(ang_pblm.mat)
l, r = pblmtx.deaug()
engine_burntimes = nnls(l, r)
i = 0
deltav = Vector(0, 0, 0)
deltaw = Vector(0, 0, 0)
for engine, ort in myShip.engines:
if max(engine_burntimes) == 0:
this_burn_time = 0
else:
this_burn_time = dT * engine_burntimes[i] / max(
engine_burntimes)
myShip.burn(engine, tanks, this_burn_time)
deltav += this_burn_time * acc[i]
deltaw += this_burn_time * ang_acc[i]
i += 1
# assuming mostly constant-acceleration.(reasonable assumption under small enough dt)
euler_vec = self.angvel * dT + 1 / 2 * deltaw * dT**2
self.ort = euler_rot(self.ort, euler_vec).unit()
self.pos += self.vel * dT + 1 / 2 * deltav.norm() * dT**2 * self.ort
# translate deltav (in ship coordinate space) into world coordinate space.
self.vel += deltav.norm() * self.ort
self.angvel += deltaw
myOrder.update_order(deltav, deltaw, dT)
def is_right_handed(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is right handed"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
return vector.dot(uv, w) > 0
def is_right_handed(p1,p2,p3,p4):
"""return True if tetrahedron p1 p2 p3 p4 is right handed"""
u = p2-p1
v = p3-p1
uv = vector.cross(u,v)
w = p4-p1
#return vector.dot(uv,w) > 0
return tol_gt(vector.dot(uv,w), 0)
def is_coplanar(p1,p2,p3,p4):
"""return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
u = p2-p1
v = p3-p1
uv = vector.cross(u,v)
w = p4-p1
#return vector.dot(uv,w) == 0
return tol_eq(vector.dot(uv,w), 0)
def getarea(self): sumA = 0.0 for i in range(0, self.totalSize()-2): v1 = self.pts[i+1] - self.pts[0] v2 = self.pts[i+2] - self.pts[0] sumA += 0.5*vector.mag(vector.cross(v1, v2)) return sumA
def is_left_handed(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is left handed"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
#return vector.dot(uv,w) < 0
return tol_lt(vector.dot(uv, w), 0)
def is_coplanar(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
#return vector.dot(uv,w) == 0
return tol_eq(vector.dot(uv, w), 0)
def vec2vec2quat(a, b):
n = vector.cross(a, b)
fac = vector.dot(a, b) / vector.norm(a) / vector.norm(b)
fac = min(max(fac, -1),
1) # protect against possible slight numerical errors
ang = math.acos(fac)
return angvec2quat(ang, n)
def __init__(self, color, pos, points):
super().__init__(color, pos)
self.mPoints = []
self.mPoints3d = []
for p in points:
v = p - self.mPos
self.mPoints.append(v)
self.mPoints3d.append(vector.Vector3(v.x, v.y, 0))
self.mArea = vector.cross(self.mPoints3d[1] - self.mPoints3d[0], self.mPoints3d[2] - self.mPoints3d[0]).magnitude / 2.0
def update(self, delta, all_boids, attractors, obstacles):
"""This is main function which figures out all the conditions and determine resultant change in the boid location per cycle. It calls all the function given above and find resultant change in location and applies it to the boid."""
#start with determining nearby boids.
self.determine_nearby_boids(all_boids)
# obstacles = self.nearby_obj(objs, _MIN_OBSTACLE_DISTANCE)
self.nearby_obj(obstacles, _MIN_OBSTACLE_DISTANCE)
#initializing all the change vectors
# cohesion_factor = self.average_position()
# alignment_factor = self.average_velocity()
# seperation_factor = self.avoid_collisions(self.neighbours, _BOID_COLLISION_DISTANCE)
p = self.all_in_one()
cohesion_factor = p[0]
alignment_factor = p[1]
seperation_factor = p[2]
collision_factor = self.avoid_collisions(self.obj_nearby,
_MIN_OBSTACLE_DISTANCE)
attraction_factor = self.attraction(attractors)
bound_factor = self.know_bound()
self.force_factors = [[_FACTOR_COHESION, cohesion_factor],
[_FACTOR_ALIGNMENT, alignment_factor],
[_FACTOR_BOID_AVOIDANCE, seperation_factor],
[_FACTOR_OBSTACLE_AVOID, collision_factor],
[_FACTOR_ATTRACT, attraction_factor],
[_FACTOR_BOUND, bound_factor]]
# print(_FACTOR_COHESION)
# print(cohesion_factor)
# y = _FACTOR_COHESION*cohesion_factor[0]
for x in self.force_factors:
self.velocity[0] += x[1][0] * x[0]
self.velocity[1] += x[0] * x[1][1] * 0.2
self.velocity[2] += x[0] * x[1][2]
velocity2 = vector.limit_magnitude(self.velocity, _SPEED_MAX,
_SPEED_MIN, True)
#changing the boids position in accordance with velocity.
for i in range(0, len(self.position)):
self.position[i] += delta * velocity2[i]
# if (self.position[i] > self.bound[i]) or (self.position[i]< -self.bound[i]):
# self.position[i] = -self.position[i]
# for i in range(3):
# if (self.position[i] > self.bound[i]) or (self.position[i]< -self.bound[i]):
# self.position = vector.limit_magnitude(self.position, self.bound[i], -self.bound[i], True)
self.force = self.mass * vector.magnitude(
self.velocity[0] - velocity2[0], self.velocity[1] - velocity2[1],
self.velocity[2] - velocity2[2]) / (self.dt * 50)
self.velocity = velocity2
cross = vector.cross(self.velocity, self.position)
self.ang_mom = self.mass * vector.magnitude(cross[0], cross[1],
cross[2]) / 1000
self.energy = self.mass * (vector.magnitude(
self.velocity[0], self.velocity[1], self.velocity[2])**2) / 4000000
def set(self, verts):
rightMost = 0
highestXCoord = verts[0].x
for i in range(len(verts)):
x = verts[i].x
if x > highestXCoord:
highestXCoord = x
rightMost = i
elif x == highestXCoord:
if verts[i].y < verts[rightMost].y:
rightMost = i
hull = [0] * Polygon.MAX_POLY_VERTEX_COUNT
outCount = 0
indexHull = rightMost
nextHullIndex = 0
while True:
hull[outCount] = indexHull
nextHullIndex = 0
for i in range(len(verts)):
if nextHullIndex == indexHull:
nextHullIndex = i
continue
e1 = verts[nextHullIndex] - verts[hull[outCount]]
e2 = verts[i] - verts[hull[outCount]]
c = cross(e1, e2)
if c < 0:
nextHullIndex = i
if c == 0 and e2.lengthSq() > e1.lengthSq():
nextHullIndex = i
outCount += 1
indexHull = nextHullIndex
if nextHullIndex == rightMost:
self.vertexCount = outCount
break
#copy vertices into shape's vertices
#print(verts)
#print("len verts:", len(verts))
#print("vertexCount:", self.vertexCount)
for i in range(self.vertexCount):
#print("i:", i)
self.vertices.append(Vec2( verts[hull[i]]))
# Compute face normals
for i in range(self.vertexCount):
face = self.vertices[(i+1)%self.vertexCount] - self.vertices[i]
self.normals.append(Vec2(face.y, -face.x))
self.normals[i].normalize()
def identity_3(v1, v2, v3):
#v1 ×(v2 × v3) = (v1 · v3)v2 − (v1 · v2)v3
v4 = vector.cross(v2, v3)
v4 = vector.cross(v1, v4) #lhs
v4 = [round(elm, 10) for elm in v4] #lhs rounded
s5 = vector.dot(v1, v3)
s6 = vector.dot(v1, v2)
s6 = -s6
v5 = vector.scalar_mult(v2, s5)
v6 = vector.scalar_mult(v3, s6)
v7 = vector.sum(v5, v6) #rhs
v7 = [round(elm, 10) for elm in v7] #rhs rounded
if v4[0]==v7[0] and v4[1]==v7[1] and v4[2]==v7[2]:
print('v1 ×(v2 × v3) = (v1 · v3)v2 − (v1 · v2)v3')
else:
print('v1 ×(v2 × v3) does not equal (v1 · v3)v2 − (v1 · v2)v3')
def intersect(self, ray):
v1 = ray.orig - self.a
v2 = self.b - self.a
v3 = Vector(-ray.dir.y, ray.dir.x)
if dot(v2, v3) == 0:
return None
ts = dot(v1, v3) / dot(v2, v3)
tr = cross(v2, v1) / dot(v2, v3)
if tr >= 0.0 and 0.0 <= ts <= 1.0:
return self.a + v2 * ts
else:
return None
def intersect(self, ray):
v1 = ray.orig - self.a
v2 = self.b - self.a
v3 = Vector(-ray.dir.y, ray.dir.x)
if dot(v2, v3) == 0:
return None
ts = dot(v1, v3) / dot(v2, v3)
tr = cross(v2, v1) / dot(v2, v3)
if tr >= 0.0 and 0.0 <= ts <= 1.0:
return self.a + v2 * ts
else:
return None
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 = []
# intersect circles in plane
cp1 = vector.vector([0.0,0.0])
cp2 = vector.vector([vector.norm(p2-p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# determine normal of plane though p1, p2, p3
n = vector.cross(p2-p1, p3-p1)
if not tol_eq(vector.norm(n),0.0):
n = n / vector.norm(n)
else:
# traingle p1, p2, p3 is degenerate
# check for 2d solutions
if len(cpxs) == 0:
return []
# project cpxs back to 3d and check radius r3
cp4 = cpxs[0]
u = normalised(p2-p1)
v,w = perp_3d(u)
p4 = p1 + cp4[0] * u + cp4[1] * v
if tol_eq(vector.norm(p4-p3), r3):
return [p4]
else:
return []
# px, rx, nx is circle
px = p1 + (p2-p1) * cpxs[0][0] / vector.norm(p2-p1)
rx = abs(cpxs[0][1])
nx = p2-p1
nx = nx / vector.norm(nx)
# py is projection of p3 on px,nx
dy3 = vector.dot(p3-px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
# ry is radius of circle in py
if tol_eq(r3,0.0):
ry = 0.0
else:
ry = math.sin(math.acos(min(1.0,abs(dy3/r3))))*r3
# determine intersection of circle px, rx and circle py, ry, projected relative to line py-px
cpx = vector.vector([0.0,0.0])
cpy = vector.vector([vector.norm(py-px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py-px) * cp4[0] / vector.norm(py-px) + n * cp4[1]
solutions.append(p4)
return solutions
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 = []
# intersect circles in plane
cp1 = vector.vector([0.0, 0.0])
cp2 = vector.vector([vector.norm(p2 - p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# determine normal of plane though p1, p2, p3
n = vector.cross(p2 - p1, p3 - p1)
if not tol_eq(vector.norm(n), 0.0):
n = n / vector.norm(n)
else:
# traingle p1, p2, p3 is degenerate
# check for 2d solutions
if len(cpxs) == 0:
return []
# project cpxs back to 3d and check radius r3
cp4 = cpxs[0]
u = normalised(p2 - p1)
v, w = perp_3d(u)
p4 = p1 + cp4[0] * u + cp4[1] * v
if tol_eq(vector.norm(p4 - p3), r3):
return [p4]
else:
return []
# px, rx, nx is circle
px = p1 + (p2 - p1) * cpxs[0][0] / vector.norm(p2 - p1)
rx = abs(cpxs[0][1])
nx = p2 - p1
nx = nx / vector.norm(nx)
# py is projection of p3 on px,nx
dy3 = vector.dot(p3 - px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
# ry is radius of circle in py
if tol_eq(r3, 0.0):
ry = 0.0
else:
ry = math.sin(math.acos(min(1.0, abs(dy3 / r3)))) * r3
# determine intersection of circle px, rx and circle py, ry, projected relative to line py-px
cpx = vector.vector([0.0, 0.0])
cpy = vector.vector([vector.norm(py - px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py - px) * cp4[0] / vector.norm(py - px) + n * cp4[1]
solutions.append(p4)
return solutions
def rayTest(self, R):
"""Returns a list of sorted Raycollisions or none if its empty."""
v = self.point2 - self.origin
w = self.point1 - self.origin
l = self.point1 - self.point2
norm = vector.cross(v, w).normalized()
P = Plane(self.material, self.origin, norm, 1000, 1000)
z = P.rayTest(R)
if z == []:
return []
A = (vector.cross(v, w) / 2).magnitude()
PBC = (vector.cross(z[0].point - self.point2, l) / 2 / A).magnitude()
PBA = (vector.cross(z[0].point - self.origin, v) / 2 / A).magnitude()
PAC = (vector.cross(z[0].point - self.origin, w) / 2 / A).magnitude()
n1 = self.origin_norm * PBC
n2 = self.point1_norm * PBA
n3 = self.point2_norm * PAC
new_norm = (n1 + n2 + n3)
if PBC + PBA + PAC <= 1.0000001:
return [Raycollision(R, self, z[0].distance, z[0].point, new_norm)]
else:
return []
def intersect(self, ray):
oc = self.center - ray.orig
# let P be projection of C on the line
# then l = |OP|, h = |CP|
l = dot(oc, ray.dir)
h = cross(oc, ray.dir)
if self.radius < abs(h):
return None
q = sqrt(self.radius ** 2 - h ** 2)
# find t: o + d * t == intersection
t = l - q
if t < 0:
t = l + q
if t < 0:
return None
else:
return ray.orig + t * ray.dir
def intersect(self, ray):
oc = self.center - ray.orig
# let P be projection of C on the line
# then l = |OP|, h = |CP|
l = dot(oc, ray.dir)
h = cross(oc, ray.dir)
if self.radius < abs(h):
return None
q = sqrt(self.radius**2 - h**2)
# find t: o + d * t == intersection
t = l - q
if t < 0:
t = l + q
if t < 0:
return None
else:
return ray.orig + t * ray.dir
def main():
# Create two random vectors
v1 = [random.random(), random.random(), random.random()]
v2 = [random.random(), random.random(), random.random()]
v3 = [random.random(), random.random(), random.random()]
#line break in output screen, clearer to read
print()
# Print out the vectors to terminal window
print("v1 = ", v1)
print("v2 = ", v2)
print("v3 = ", v3)
#line break in output screen, clearer to read
print()
#print basic vector manipulations
#vector sum, scalar product and vector (cross) product
print("v1 + v2 =", vector.sum(v1, v2))
print('v1 · v2 =', vector.dot(v1, v2))
print('v1 x v2 =', vector.cross(v1, v2))
#line break in output screen, clearer to read
print()
#testing if the vector identites hold
identity_1(v1, v2)
identity_2(v1, v2, v3)
identity_3(v1, v2, v3)
def compare_square_normals(self, square, dim):
"""
Increments the score of two squares based on how parallel their normal
vectors are.
:param square:
:param dim: the dimensions of the image
:return: nothing
"""
# Using 1-cross product due to larger change of sin when parallel
temp_cross = cross(self.normal, square.normal)
edge = False
for point in square.corners:
if point[0][0] < 1 or point[0][0] > dim[1] - 2 or point[0][1] < 1 \
or point[0][1] > dim[0] - 2:
# Contours on the edge don't improve scores
edge = True
if not edge:
# Increment the score
self.score += 1 - abs(distance(temp_cross) / (distance(square.normal)*distance(self.normal)))
def computeMass(self, density, body):
centroid = Vec2()
area = 0
triangleArea = 0
inertia = 0
k_inv6 = 1.0 / 6
#count centroid and inertia
for i in range(self.vertexCount):
p1 = self.vertices[i]
p2 = self.vertices[ (i+1) % self.vertexCount]
D = cross(p1, p2)
triangleArea = 0.5 * D
area += triangleArea
#print(area)
centroid += (p1 * D)
centroid += (p2 * D)
intx2 = p1.x ** 2 + p2.x * p1.x + p2.x ** 2
inty2 = p1.y ** 2 + p2.y * p1.y + p2.y ** 2
inertia += (0.5 * k_inv6 * D) * (intx2+ inty2)
centroid = centroid * k_inv6 / area
#translate vertices to centroid
for i in range(self.vertexCount):
self.vertices[i] -= centroid
body.mass = density * area
body.invMass = 1 / body.mass if body.mass else 0
body.inertia = inertia * density
body.invInertia = 1 / body.inertia if body.inertia else 0
self.body = body
def compare_square_normals(self, square, dim):
"""
Increments the score of two squares based on how parallel their normal
vectors are.
:param square:
:param dim: the dimensions of the image
:return: nothing
"""
# Using 1-cross product due to larger change of sin when parallel
temp_cross = cross(self.normal, square.normal)
edge = False
for point in square.corners:
if point[0][0] < 1 or point[0][0] > dim[1] - 2 or point[0][1] < 1 \
or point[0][1] > dim[0] - 2:
# Contours on the edge don't improve scores
edge = True
if not edge:
# Increment the score
self.score += 1 - abs(
distance(temp_cross) /
(distance(square.normal) * distance(self.normal)))
def test_vec_cross(self):
v1 = Vector(1, 2, 3)
v2 = Vector(4, 5, 6)
v3 = Vector(-3, 6, -3)
self.assertEqual(v1 % v2, v3)
self.assertEqual(cross(v1, v2), v3)
def distance_to_segment(point, a, b):
if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0:
return min(dist(point, a), dist(point, b))
return abs(cross(point - a, normalize(b - a)))
def test_vec_cross(self):
v1 = Vector( 1, 2, 3)
v2 = Vector( 4, 5, 6)
v3 = Vector(-3, 6, -3)
self.assertEqual(v1 % v2, v3)
self.assertEqual(cross(v1, v2), v3)
def perp_3d(u):
"""Returns 2 orthonormal vectors."""
t = transform_point(u, perp_matrix)
v = normalised(vector.cross(u,t))
w = normalised(vector.cross(u,v))
return (v,w)
def test_mul_cross_normal(self):
n1 = (1., 0, 0)
n2 = (0, 1., 0)
n3 = (0, 0, 1.)
self.assertEqual(cross(n1, n2), n3)
def view_up(self):
return (vector.cross(self.viewing_direction,
self.view_left)).normalized
def view_left(self):
return (vector.cross(self._up_vector,
self.viewing_direction)).normalized
def test_mul_cross(self):
v1 = [1, 2, 3]
v2 = [4, 5, 6]
v3 = [-3, 6, -3]
self.assertEqual(cross(v1, v2), v3)
def test_mul_cross_zero(self):
n = [0] * 3
self.assertEqual(cross(n, n), n)
#!/usr/bin/env python2
import vector
import math
v1 = 1,2,3
v2 = 3,4,5
n1 = vector.normalize(v1)
assert vector.add(v1, v2) == (4, 6, 8)
assert vector.subtract(v1, v2) == (-2, -2, -2)
assert abs(n1[0] - 0.27) < 0.01 and \
abs(n1[1] - 0.53) < 0.01 and \
abs(n1[2] - 0.80) < 0.01
assert vector.dot(v1, v2) == 26
assert vector.cross(v1, v2) == (-2, 4, -2)
print "All tests passed!"
def test_mul_cross_zero(self):
n = [0] * 3
self.assertEqual(cross(n, n), n)
def test_mul_cross_normal(self):
n1 = [1., 0, 0]
n2 = [0, 1., 0]
n3 = [0, 0, 1.]
self.assertEqual(cross(n1, n2), n3)
def perp_3d(u):
"""Returns 2 orthonormal vectors."""
t = transform_point(u, perp_matrix)
v = normalised(vector.cross(u, t))
w = normalised(vector.cross(u, v))
return (v, w)
def square2(p1, p2, p3):
return vec.cross(p2 - p1, p3 - p1)
def test_vec_cross_normal(self):
n1 = Vector(1., 0, 0)
n2 = Vector(0, 1., 0)
n3 = Vector(0, 0, 1.)
self.assertEqual(n1 % n2, n3)
self.assertEqual(cross(n1, n2), n3)
def mul(P, Q):
w1, v1 = P
w2, v2 = Q
return (w1*w2 - _v.dot(v1, v2),_v.sum(_v.mul(w1, v2), _v.mul(w2, v1), _v.cross(v1, v2)))
def intersectedArea(self, g, Debug=False):
if not self.intersect(g):
return -1
#--- search the pivot point
ist = 0
poly = []
for i in range(0, self.size()):
if g.inside(self.pts[i]):
poly.append(self.pts[ist])
ist = i
break
lseg = lineseg.lineseg(self.pts[i], self.pts[i+1])
isectd, psect, iseg = g.intersect(lseg)
if isectd:
poly.append(psect)
ist = i
break
#--- Walk along boundary of intersected area
# cag : cell as geometry
cag = geometry(self.points(False))
# Loop over the cell sides
for i in range(ist+1, self.size()+1):
p = self.pts[i]
# f: a side of this cell
f = lineseg.lineseg(self.pts[i-1], self.pts[i])
# Check if the side f intersect with given geometry g
# isectd: boolean if g and f intersected or not
# psect : intersected point
# iseg : index of intersected segment of g
isectd, psect, iseg = g.intersect(f)
if isectd:
# Check if psect has been added to poly already
if not iselem(psect, poly):
poly.append(psect)
# Walk along the geometry g to find another intersect point of
# this cell and g. The geometry points inbetween the two
# intersected points are added to the poly
for i in range(iseg+1, g.size()):
pchk = g.pts[i]
if cag.inside(pchk) and \
not iselem(pchk, poly):
poly.append(pchk)
if g.inside(p) and not iselem(p, poly):
poly.append(p)
if Debug:
print('\nPoints in this poly:')
for l in poly:
print(l)
# Estimate the area
if len(poly) == 3:
v1 = poly[1] - poly[0]
v2 = poly[2] - poly[1]
return 0.5*vector.mag(vector.cross(v1, v2))
else:
sumA = 0.0
for i in range(0, len(poly)-2):
v1 = poly[i+1] - poly[0]
v2 = poly[i+2] - poly[0]
sumA += 0.5*vector.mag(vector.cross(v1, v2))
return sumA
#!/usr/bin/env python2 import vector import math v1 = [36.886, 53.177, 21.887] v2 = [38.323, 52.817, 21.996] v3 = [38.493, 51.553, 22.830] v4 = [39.483, 50.748, 22.463] n1 = vector.normalize(v1) n2 = vector.normalize(v2) v1 = n1 = 1,2,3 v2 = n2 = 3,4,5 print vector.add(v1, v2) print vector.subtract(v1, v2) print vector.normalize(v1) print vector.dot(n1, n2) print vector.cross(n1, n2) print vector.torsion(v1, v2, v3, v4)
def test_vec_cross_zero(self):
n = Vector(0, 0, 0)
self.assertEqual(n % n, n)
self.assertEqual(cross(n, n), n)
def test_mul_cross(self):
v1 = [ 1, 2, 3]
v2 = [ 4, 5, 6]
v3 = [-3, 6, -3]
self.assertEqual(cross(v1, v2), v3)
def applyImpulse(self, impulse, contactVector):
self.velocity += impulse * self.invMass
self.angularVelocity += self.invInertia * cross(contactVector, impulse)
def distance_to_segment(point, a, b):
if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0:
return min(dist(point, a), dist(point, b))
return abs(cross(point - a, normalize(b - a)))
def FitPointsCompass(debug, points, current, norm):
# ensure current and norm are float
current = lmap(float, current)
norm = lmap(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = lmap(lambda x : x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
debug('0d fit, insufficient data %.1f %.1f < 9' % (point_dev, point_max_dev))
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
plane_fit, plane_dev, plane_max_dev = plane
# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = lmap(min, p[:3], minc)
maxc = lmap(max, p[:3], maxc)
guess = lmap(lambda a, b : (a+b)/2, minc, maxc)
diff = lmap(lambda a, b : b-a, minc, maxc)
guess.append((diff[0]+diff[1]+diff[2])/3)
#debug('initial guess', guess)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
#debug('initial 1d fit', initial)
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] < 0:
print('FitLeastSq sphere1d failed!!!! ', len(points))
return False
sphere1d_fit = lmap(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
debug('sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit))
'''
def f_new_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints, debug, 2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
new_sphere1d_fit = current
else:
new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
#print('new sphere1 fit', new_sphere1d_fit)
if line_max_dev < 2:
debug('line fit found, insufficient data %.1f %.1f' % (line_dev, line_max_dev))
return False
# 2d sphere fit across normal vector
u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
#debug('initial 2d fit', initial)
'''
def f_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] < 0:
print('FitLeastSq sphere2d failed!!!! ', len(points))
new_sphere2d_fit = initial
else:
sphere2d_fit = lmap(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
debug('sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit))
'''
def f_new_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[2], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2, zpoints, debug, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0 or abs(new_sphere2d_fit[3]) >= 1:
debug('FitLeastSq sphere2 failed!!!! ', len(points), new_sphere2d_fit)
return False
new_sphere2d_fit = lmap(lambda x, a, b: x + new_sphere2d_fit[0]*a + new_sphere2d_fit[1]*b, initial[:3], u, v) + [new_sphere2d_fit[2], math.degrees(math.asin(new_sphere2d_fit[3]))]
new_sphere2d_fit = [new_sphere2d_fit, ComputeDeviation(points, new_sphere2d_fit), 2]
if plane_max_dev < 1.2:
ang = math.degrees(math.asin(vector.norm(vector.cross(plane_fit[1], norm))))
debug('plane fit found, 2D fit only', ang, plane_fit, plane_dev, plane_max_dev)
if ang > 30:
debug('angle of plane not aligned to normal: no 2d fit')
new_sphere2d_fit = False
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print('FitLeastSq sphere failed!!!! ', len(points))
return False
debug('sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit))
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, debug, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0 or abs(new_sphere3d_fit[4]) >= 1:
debug('FitLeastSq sphere3 failed!!!! ', len(points))
return False
new_sphere3d_fit[4] = math.degrees(math.asin(new_sphere3d_fit[4]))
new_sphere3d_fit = [new_sphere3d_fit, ComputeDeviation(points, new_sphere3d_fit), 3]
#debug('new sphere3 fit', new_sphere3d_fit)
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def FitPoints(points, current, norm):
if len(points) < 5:
return False
# ensure current and norm are float
current = map(float, current)
norm = map(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = map(lambda x: x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
if debug:
print '0d fit, insufficient data', point_dev, point_max_dev, '< 9'
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
plane_fit, plane_dev, plane_max_dev = plane
# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = map(min, p[:3], minc)
maxc = map(max, p[:3], maxc)
guess = map(lambda a, b: (a + b) / 2, minc, maxc)
diff = map(lambda a, b: b - a, minc, maxc)
guess.append((diff[0] + diff[1] + diff[2]) / 3)
if debug:
print 'initial guess', guess
# initial is the closest to guess on the uv plane containing current
initial = vector.add(
current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
if debug:
print 'initial 1d fit', initial
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] < 0:
print 'FitLeastSq sphere1d failed!!!! ', len(points)
return False
sphere1d_fit = map(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
if debug:
print 'sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit)
'''
def f_new_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0] * n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[1] * (beta[2] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints,
2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0:
if debug:
print 'FitLeastSq new_sphere1 failed!!!! ', len(points)
new_sphere1d_fit = current
else:
new_sphere1d_fit = map(lambda x, a: x + new_sphere1d_fit[0] * a,
initial[:3], norm) + new_sphere1d_fit[1:]
new_sphere1d_fit = [
new_sphere1d_fit,
ComputeDeviation(points, new_sphere1d_fit), 1
]
#print 'new sphere1 fit', new_sphere1d_fit
if line_max_dev < 3:
if debug:
print 'line fit found, insufficient data', line_dev, line_max_dev
return [new_sphere1d_fit, False, False]
# 2d sphere fit across normal vector
u = vector.cross(norm,
[norm[1] - norm[2], norm[2] - norm[0], norm[0] - norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(
guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
if debug:
print 'initial 2d fit', initial
'''
def f_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] < 0:
print 'FitLeastSq sphere2d failed!!!! ', len(points)
new_sphere2d_fit = initial
else:
sphere2d_fit = map(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
if debug:
print 'sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit)
'''
def f_new_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0] * a + beta[1] * b, initial[:3],
u, v)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[2] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[2] * (beta[3] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2,
zpoints, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere2d_fit = map(
lambda x, a, b: x + new_sphere2d_fit[0] * a + new_sphere2d_fit[1] * b,
initial[:3], u, v) + new_sphere2d_fit[2:]
new_sphere2d_fit = [
new_sphere2d_fit,
ComputeDeviation(points, new_sphere2d_fit), 2
]
#print 'new sphere2 fit', new_sphere2d_fit
if plane_max_dev < 1.2:
if debug:
print 'plane fit found, 2D fit only', plane_fit, plane_dev, plane_max_dev
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
if debug:
print 'sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit)
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(map(lambda a, b: a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[3] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[3] * (beta[4] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere3d_fit = [
new_sphere3d_fit,
ComputeDeviation(points, new_sphere3d_fit), 3
]
#print 'new sphere3 fit', new_sphere3d_fit
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def test_mul_cross(self):
v1 = ( 1, 2, 3)
v2 = ( 4, 5, 6)
v3 = (-3, 6, -3)
self.assertEqual(cross(v1, v2), v3)
def test_mul_cross_normal(self):
n1 = [1., 0, 0]
n2 = [0, 1., 0]
n3 = [0, 0, 1.]
self.assertEqual(cross(n1, n2), n3)
def test_mul_cross_zero(self):
n = (0, 0, 0)
self.assertEqual(cross(n, n), n)
def test_vec_cross_normal(self):
n1 = Vector(1., 0, 0)
n2 = Vector(0, 1., 0)
n3 = Vector(0, 0, 1.)
self.assertEqual(n1 % n2, n3)
self.assertEqual(cross(n1, n2), n3)