def mouseMoved(x, y):
global xrot, yrot, zrot, xdist, ydist, zdist, scale
global width, height
x = float(x)
y = float(y)
a1 = math.atan2(mouseState.y-height/2.0, mouseState.x-width/2.0)
r1 = sqrt((mouseState.y - height / 2.0) ** 2 \
+ (mouseState.x - width / 2.0) ** 2)
a2 = math.atan2(y-height/2.0, x-width/2.0)
r2 = sqrt((y - height / 2.0) ** 2 + (x - width / 2.0) ** 2)
if (mouseState.button == GLUT.GLUT_LEFT_BUTTON) \
or (mouseState.button == GLUT.GLUT_RIGHT_BUTTON):
a3 = math.acos(mouseState.x/width-0.5)
a4 = math.acos(x/width-0.5)
zrot = zrot - (a4-a3)*180/math.pi*2
if mouseState.button == GLUT.GLUT_RIGHT_BUTTON:
a3 = math.acos(mouseState.y/height-0.5)
a4 = math.acos(y/height-0.5)
if x > width / 2.0:
yrot = yrot + (a4-a3)*180/math.pi*2
else:
yrot = yrot - (a4-a3)*180/math.pi*2
if mouseState.button == GLUT.GLUT_LEFT_BUTTON:
a3 = math.acos(mouseState.y/width-0.5)
a4 = math.acos(y/width-0.5)
xrot = xrot - (a4-a3)*180/math.pi*2
mouseState.x = x
mouseState.y = y
def add_wave(self, freq=8, damp=3.0):
self.changed = True
rmax = sqrt(self.y[0] * self.y[0] + self.x[0] * self.x[0])
for y in range(0, self.yres):
for x in range(0, self.xres):
r = sqrt(self.y[y] * self.y[y] + self.x[x] * self.x[x])
self.buf[y][x].z = 1 + math.cos(r / rmax * r / rmax * math.pi \
* freq) / (1 + damp * (r / rmax))
self.buf[y][x].changed = True
def add_wave(self, freq=8, damp=3.0):
self.changed = True
rmax = sqrt(self.y[0]*self.y[0]+self.x[0]*self.x[0])
for y in range(0, self.yres):
for x in range(0, self.xres):
r = sqrt(self.y[y]*self.y[y]+self.x[x]*self.x[x])
self.buf[y][x].z = 1 + math.cos(r / rmax * r / rmax * math.pi \
* freq) / (1 + damp * (r / rmax))
self.buf[y][x].changed = True
def intersect_torus_point(center, axis, majorradius, minorradius, majorradiussq,
minorradiussq, direction, point):
dist = 0
if (direction.x == 0) and (direction.y == 0):
# drop
minlsq = (majorradius - minorradius) ** 2
maxlsq = (majorradius + minorradius) ** 2
l_sq = (point.x-center.x) ** 2 + (point.y - center.y) ** 2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l) ** 2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = Point(point.x, point.y, center.z - z)
dist = ccp.z - point.z
elif direction.z == 0:
# push
z = point.z - center.z
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l = majorradius + sqrt(minorradiussq - z * z)
n = axis.cross(direction)
d = n.dot(point) - n.dot(center)
if abs(d) > l - epsilon:
return (None, None, INFINITE)
a = sqrt(l * l - d * d)
ccp = center.add(n.mul(d).add(direction.mul(a)))
ccp.z = point.z
dist = point.sub(ccp).dot(direction)
else:
# general case
x = point.sub(center)
v = direction.mul(-1)
x_x = x.dot(x)
x_v = x.dot(v)
x1 = Point(x.x, x.y, 0)
v1 = Point(v.x, v.y, 0)
x1_x1 = x1.dot(x1)
x1_v1 = x1.dot(v1)
v1_v1 = v1.dot(v1)
R2 = majorradiussq
r2 = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v ** 2 + (R2 - r2) - 2 * R2 * v1_v1)
d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
e = (x_x) ** 2 + 2 * x_x * (R2 - r2) + (R2 - r2) ** 2 - 4 * R2 * x1_x1
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l = min(r)
ccp = point.add(direction.mul(-l))
dist = l
return (ccp, point, dist)
def intersect_torus_point(center, axis, majorradius, minorradius,
majorradiussq, minorradiussq, direction, point):
dist = 0
if (direction.x == 0) and (direction.y == 0):
# drop
minlsq = (majorradius - minorradius)**2
maxlsq = (majorradius + minorradius)**2
l_sq = (point.x - center.x)**2 + (point.y - center.y)**2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l)**2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = Point(point.x, point.y, center.z - z)
dist = ccp.z - point.z
elif direction.z == 0:
# push
z = point.z - center.z
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l = majorradius + sqrt(minorradiussq - z * z)
n = axis.cross(direction)
d = n.dot(point) - n.dot(center)
if abs(d) > l - epsilon:
return (None, None, INFINITE)
a = sqrt(l * l - d * d)
ccp = center.add(n.mul(d).add(direction.mul(a)))
ccp.z = point.z
dist = point.sub(ccp).dot(direction)
else:
# general case
x = point.sub(center)
v = direction.mul(-1)
x_x = x.dot(x)
x_v = x.dot(v)
x1 = Point(x.x, x.y, 0)
v1 = Point(v.x, v.y, 0)
x1_x1 = x1.dot(x1)
x1_v1 = x1.dot(v1)
v1_v1 = v1.dot(v1)
R2 = majorradiussq
r2 = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v**2 + (R2 - r2) - 2 * R2 * v1_v1)
d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
e = (x_x)**2 + 2 * x_x * (R2 - r2) + (R2 - r2)**2 - 4 * R2 * x1_x1
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l = min(r)
ccp = point.add(direction.mul(-l))
dist = l
return (ccp, point, dist)
def intersect_torus_point(center, axis, majorradius, minorradius, majorradiussq,
minorradiussq, direction, point):
dist = 0
if (direction[0] == 0) and (direction[1] == 0):
# drop
minlsq = (majorradius - minorradius) ** 2
maxlsq = (majorradius + minorradius) ** 2
l_sq = (point[0]-center[0]) ** 2 + (point[1] - center[1]) ** 2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l) ** 2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = (point[0], point[1], center[2] - z)
dist = ccp[2] - point[2]
elif direction[2] == 0:
# push
z = point[2] - center[2]
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l = majorradius + sqrt(minorradiussq - z * z)
n = pcross(axis, direction)
d = pdot(n, point) - pdot(n, center)
if abs(d) > l - epsilon:
return (None, None, INFINITE)
a = sqrt(l * l - d * d)
ccp = padd(padd(center, pmul(n, d)), pmul(direction, a))
ccp = (ccp[0], ccp[1], point[2])
dist = pdot(psub(point, ccp), direction)
else:
# general case
x = psub(point, center)
v = pmul(direction, -1)
x_x = pdot(x, x)
x_v = pdot(x, v)
x1 = (x[0], x[1], 0)
v1 = (v[0], v[1], 0)
x1_x1 = pdot(x1, x1)
x1_v1 = pdot(x1, v1)
v1_v1 = pdot(v1, v1)
R2 = majorradiussq
r2 = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v ** 2 + (R2 - r2) - 2 * R2 * v1_v1)
d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
e = (x_x) ** 2 + 2 * x_x * (R2 - r2) + (R2 - r2) ** 2 - 4 * R2 * x1_x1
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l = min(r)
ccp = padd(point, pmul(direction, -l))
dist = l
return (ccp, point, dist)
def do_pavement(self) :
self.rad = self.diameter/2
self.apo = sqrt(3)/2*self.rad
self.apo2 = self.apo*2
self.rad05 = self.rad*0.5
self.rad15 = self.rad*1.5
self.slope = 2./sqrt(3.)
self.nb_lines = int((self.model.maxx - self.model.minx + self.apo) // self.apo2)+1
self.nb_columns = int((self.model.maxy - self.model.miny + self.rad05) // self.rad15)+1
self.height = self.diameter # TODO: take account of overlapping, ...
def do_pavement(self):
self.rad = self.diameter / 2
self.apo = sqrt(3) / 2 * self.rad
self.apo2 = self.apo * 2
self.rad05 = self.rad * 0.5
self.rad15 = self.rad * 1.5
self.slope = 2. / sqrt(3.)
self.nb_lines = int(
(self.model.maxx - self.model.minx + self.apo) // self.apo2) + 1
self.nb_columns = int(
(self.model.maxy - self.model.miny + self.rad05) // self.rad15) + 1
self.height = self.diameter # TODO: take account of overlapping, ...
def move_camera_by_screen(self, x_move, y_move, max_model_shift):
""" move the camera acoording to a mouse movement
@type x_move: int
@value x_move: movement of the mouse along the x axis
@type y_move: int
@value y_move: movement of the mouse along the y axis
@type max_model_shift: float
@value max_model_shift: maximum shifting of the model view (e.g. for
x_move == screen width)
"""
factors_x, factors_y = self._get_axes_vectors()
width, height = self._get_screen_dimensions()
# relation of x/y movement to the respective screen dimension
win_x_rel = (-2 * x_move) / float(width) / math.sin(self.view["fovy"])
win_y_rel = (-2 * y_move) / float(height) / math.sin(self.view["fovy"])
# This code is completely arbitrarily based on trial-and-error for
# finding a nice movement speed for all distances.
# Anyone with a better approach should just fix this.
distance_vector = self.get("distance")
distance = float(sqrt(sum([dim**2 for dim in distance_vector])))
win_x_rel *= math.cos(win_x_rel / distance)**20
win_y_rel *= math.cos(win_y_rel / distance)**20
# update the model position that should be centered on the screen
old_center = self.view["center"]
new_center = []
for i in range(3):
new_center.append(old_center[i] \
+ max_model_shift * (number(win_x_rel) * factors_x[i] \
+ number(win_y_rel) * factors_y[i]))
self.view["center"] = tuple(new_center)
def intersect_sphere_line(center, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
n = d.cross(direction)
if n.norm == 0:
# no contact point, but should check here if sphere *always* intersects
# line...
return (None, None, INFINITE)
n = n.normalized()
# calculate the distance from the sphere center to the plane
dist = -center.dot(n) + edge.p1.dot(n)
if abs(dist) > radius - epsilon:
return (None, None, INFINITE)
# this gives us the intersection circle on the sphere
# now take a plane through the edge and perpendicular to the direction (2)
# find the center on the circle closest to this plane
# which means the other component is perpendicular to this plane (2)
n2 = n.cross(d).normalized()
# the contact point is on a big circle through the sphere...
dist2 = sqrt(radiussq - dist * dist)
# ... and it's on the plane (1)
ccp = center.add(n.mul(dist)).add(n2.mul(dist2))
# now intersect a line through this point with the plane (2)
plane = Plane(edge.p1, n2)
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def move_camera_by_screen(self, x_move, y_move, max_model_shift):
""" move the camera acoording to a mouse movement
@type x_move: int
@value x_move: movement of the mouse along the x axis
@type y_move: int
@value y_move: movement of the mouse along the y axis
@type max_model_shift: float
@value max_model_shift: maximum shifting of the model view (e.g. for
x_move == screen width)
"""
factors_x, factors_y = self._get_axes_vectors()
width, height = self._get_screen_dimensions()
# relation of x/y movement to the respective screen dimension
win_x_rel = (-2 * x_move) / float(width) / math.sin(self.view["fovy"])
win_y_rel = (-2 * y_move) / float(height) / math.sin(self.view["fovy"])
# This code is completely arbitrarily based on trial-and-error for
# finding a nice movement speed for all distances.
# Anyone with a better approach should just fix this.
distance_vector = self.get("distance")
distance = float(sqrt(sum([dim ** 2 for dim in distance_vector])))
win_x_rel *= math.cos(win_x_rel / distance) ** 20
win_y_rel *= math.cos(win_y_rel / distance) ** 20
# update the model position that should be centered on the screen
old_center = self.view["center"]
new_center = []
for i in range(3):
new_center.append(old_center[i] \
+ max_model_shift * (number(win_x_rel) * factors_x[i] \
+ number(win_y_rel) * factors_y[i]))
self.view["center"] = tuple(new_center)
def intersect_sphere_line(center, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
n = pcross(d, direction)
if pnorm(n) == 0:
# no contact point, but should check here if sphere *always* intersects
# line...
return (None, None, INFINITE)
n = pnormalized(n)
# calculate the distance from the sphere center to the plane
dist = - pdot(center, n) + pdot(edge.p1, n)
if abs(dist) > radius - epsilon:
return (None, None, INFINITE)
# this gives us the intersection circle on the sphere
# now take a plane through the edge and perpendicular to the direction (2)
# find the center on the circle closest to this plane
# which means the other component is perpendicular to this plane (2)
n2 = pnormalized(pcross(n, d))
# the contact point is on a big circle through the sphere...
dist2 = sqrt(radiussq - dist * dist)
# ... and it's on the plane (1)
ccp = padd(center, padd(pmul(n, dist), pmul(n2, dist2)))
# now intersect a line through this point with the plane (2)
plane = Plane(edge.p1, n2)
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of the
# PushCutter). We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCapsule(None, radius, self.height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects the two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom((
Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius,
self.height - diff_z / 2),
Point(hypotenuse / 2, radius,
self.height + diff_z / 2),
Point(hypotenuse / 2, -radius,
self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# Create a cylinder, that connects the two half spheres at the
# lower end of both drills.
geom_cyl_transform = ode.GeomTransform(geom.space)
geom_cyl_transform.setBody(geom.getBody())
hypotenuse_3d = Matrix.get_length((diff_x, diff_y, diff_z))
geom_cyl = ode.GeomCylinder(None, radius, hypotenuse_3d)
# rotate cylinder vector
cyl_original_vector = (0, 0, hypotenuse_3d)
cyl_destination_vector = (diff_x, diff_y, diff_z)
matrix = Matrix.get_rotation_matrix_from_to(
cyl_original_vector, cyl_destination_vector)
flat_matrix = matrix[0] + matrix[1] + matrix[2]
geom_cyl.setRotation(flat_matrix)
# The rotation is around the center - thus we ignore negative
# diff values.
geom_cyl.setPosition((abs(diff_x / 2), abs(diff_y / 2),
radius - additional_distance))
geom_cyl_transform.setGeom(geom_cyl)
# sort the geoms in order of collision probability
geom.children.extend([geom_connect_transform,
geom_cyl_transform, geom_end_transform])
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of the
# PushCutter). We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCapsule(None, radius, self.height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects the two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
Point(hypotenuse / 2, radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# Create a cylinder, that connects the two half spheres at the
# lower end of both drills.
geom_cyl_transform = ode.GeomTransform(geom.space)
geom_cyl_transform.setBody(geom.getBody())
hypotenuse_3d = Matrix.get_length((diff_x, diff_y, diff_z))
geom_cyl = ode.GeomCylinder(None, radius, hypotenuse_3d)
# rotate cylinder vector
cyl_original_vector = (0, 0, hypotenuse_3d)
cyl_destination_vector = (diff_x, diff_y, diff_z)
matrix = Matrix.get_rotation_matrix_from_to(
cyl_original_vector, cyl_destination_vector)
flat_matrix = matrix[0] + matrix[1] + matrix[2]
geom_cyl.setRotation(flat_matrix)
# The rotation is around the center - thus we ignore negative
# diff values.
geom_cyl.setPosition((abs(diff_x / 2), abs(diff_y / 2),
radius - additional_distance))
geom_cyl_transform.setGeom(geom_cyl)
# sort the geoms in order of collision probability
geom.children.extend([
geom_connect_transform, geom_cyl_transform,
geom_end_transform
])
def get_length(vector):
""" calculate the lengt of a 3d vector
@type vector: tuple(float) | list(float)
@value vector: the given 3d vector
@rtype: float
@return: the length of a vector is the square root of the dot product
of the vector with itself
"""
return sqrt(get_dot_product(vector, vector))
def get_length(vector):
""" calculate the lengt of a 3d vector
@type vector: tuple(float) | list(float)
@value vector: the given 3d vector
@rtype: float
@return: the length of a vector is the square root of the dot product
of the vector with itself
"""
return sqrt(get_dot_product(vector, vector))
def intersect_sphere_point(center, radius, radiussq, direction, point):
# line equation
# (1) x = p_0 + \lambda * d
# sphere equation
# (2) (x-x_0)^2 = R^2
# (1) in (2) gives a quadratic in \lambda
p0_x0 = center.sub(point)
a = direction.normsq
b = 2 * p0_x0.dot(direction)
c = p0_x0.normsq - radiussq
d = b * b - 4 * a * c
if d < 0:
return (None, None, INFINITE)
if a < 0:
l = (-b + sqrt(d)) / (2 * a)
else:
l = (-b - sqrt(d)) / (2 * a)
# cutter contact point
ccp = point.add(direction.mul(-l))
return (ccp, point, l)
def intersect_sphere_point(center, radius, radiussq, direction, point):
# line equation
# (1) x = p_0 + \lambda * d
# sphere equation
# (2) (x-x_0)^2 = R^2
# (1) in (2) gives a quadratic in \lambda
p0_x0 = psub(center, point)
a = pnormsq(direction)
b = 2 * pdot(p0_x0, direction)
c = pnormsq(p0_x0) - radiussq
d = b * b - 4 * a * c
if d < 0:
return (None, None, INFINITE)
if a < 0:
l = (-b + sqrt(d)) / (2 * a)
else:
l = (-b - sqrt(d)) / (2 * a)
# cutter contact point
ccp = padd(point, pmul(direction, -l))
return (ccp, point, l)
def intersect_cylinder_point(center, axis, radius, radiussq, direction, point):
# take a plane along direction and axis
n = direction.cross(axis).normalized()
# distance of the point to this plane
d = n.dot(point) - n.dot(center)
if abs(d) > radius - epsilon:
return (None, None, INFINITE)
# ccl is on cylinder
d2 = sqrt(radiussq - d * d)
ccl = center.add(n.mul(d)).add(direction.mul(d2))
# take plane through ccl and axis
plane = Plane(ccl, direction)
# intersect point with plane
(ccp, l) = plane.intersect_point(direction, point)
return (ccp, point, -l)
def intersect_cylinder_point(center, axis, radius, radiussq, direction, point):
# take a plane along direction and axis
n = pnormalized(pcross(direction, axis))
# distance of the point to this plane
d = pdot(n, point) - pdot(n, center)
if abs(d) > radius - epsilon:
return (None, None, INFINITE)
# ccl is on cylinder
d2 = sqrt(radiussq-d*d)
ccl = padd( padd(center, pmul(n, d)), pmul(direction, d2))
# take plane through ccl and axis
plane = Plane(ccl, direction)
# intersect point with plane
(ccp, l) = plane.intersect_point(direction, point)
return (ccp, point, -l)
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1):
# convert p1 and p2 from list/tuple to Point
if not hasattr(p1, "sub"):
p1 = Point(*p1)
if not hasattr(p2, "sub"):
p2 = Point(*p2)
distance = p2.sub(p1)
length = distance.norm
direction = distance.normalized()
if direction is None:
# zero-length line
return
cone_length = length * size
cone_radius = cone_length / 3.0
# move the cone to the middle of the line
GL.glTranslatef((p1.x + p2.x) * position, (p1.y + p2.y) * position,
(p1.z + p2.z) * position)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = direction.cross(Point(0, 0, -1))
if cross.norm != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction.x**2 + direction.y**2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction.z < 0:
angle = 180 - angle
GL.glRotatef(angle, cross.x, cross.y, cross.z)
elif direction.z == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length * position)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1):
# convert p1 and p2 from list/tuple to Point
if not hasattr(p1, "sub"):
p1 = Point(*p1)
if not hasattr(p2, "sub"):
p2 = Point(*p2)
distance = p2.sub(p1)
length = distance.norm
direction = distance.normalized()
if direction is None:
# zero-length line
return
cone_length = length * size
cone_radius = cone_length / 3.0
# move the cone to the middle of the line
GL.glTranslatef((p1.x + p2.x) * position,
(p1.y + p2.y) * position, (p1.z + p2.z) * position)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = direction.cross(Point(0, 0, -1))
if cross.norm != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction.x ** 2 + direction.y ** 2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction.z < 0:
angle = 180 - angle
GL.glRotatef(angle, cross.x, cross.y, cross.z)
elif direction.z == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length * position)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
triangle):
# take normal to the plane
n = triangle.normal
if n.dot(direction) == 0:
return (None, None, INFINITE)
if n.dot(axis) == 1:
return (None, None, INFINITE)
# find place on torus where surface normal is n
b = n.mul(-1)
z = axis
a = b.sub(z.mul(z.dot(b)))
a_sq = a.normsq
if a_sq <= 0:
return (None, None, INFINITE)
a = a.div(sqrt(a_sq))
ccp = center.add(a.mul(majorradius)).add(b.mul(minorradius))
# find intersection with plane
(cp, l) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
triangle):
# take normal to the plane
n = triangle.normal
if pdot(n, direction) == 0:
return (None, None, INFINITE)
if pdot(n, axis) == 1:
return (None, None, INFINITE)
# find place on torus where surface normal is n
b = pmul(n, -1)
z = axis
a = psub(b, pmul(z,pdot(z, b)))
a_sq = pnormsq(a)
if a_sq <= 0:
return (None, None, INFINITE)
a = pdiv(a, sqrt(a_sq))
ccp = padd(padd(center, pmul(a, majorradius)), pmul(b, minorradius))
# find intersection with plane
(cp, l) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
triangle):
# take normal to the plane
n = triangle.normal
if n.dot(direction) == 0:
return (None, None, INFINITE)
if n.dot(axis) == 1:
return (None, None, INFINITE)
# find place on torus where surface normal is n
b = n.mul(-1)
z = axis
a = b.sub(z.mul(z.dot(b)))
a_sq = a.normsq
if a_sq <= 0:
return (None, None, INFINITE)
a = a.div(sqrt(a_sq))
ccp = center.add(a.mul(majorradius)).add(b.mul(minorradius))
# find intersection with plane
(cp, l) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
((-hypotenuse / 2, radius, -diff_z / 2),
(hypotenuse / 2, radius, diff_z / 2),
(hypotenuse / 2, -radius, diff_z / 2),
(-hypotenuse / 2, -radius, -diff_z / 2)),
((-hypotenuse / 2, radius,
self.height - diff_z / 2),
(hypotenuse / 2,
radius, self.height + diff_z / 2),
(hypotenuse / 2, -radius,
self.height + diff_z / 2),
(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend([geom_connect_transform,
geom_end_transform])
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius,
self.height - diff_z / 2),
Point(hypotenuse / 2,
radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius,
self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend([geom_connect_transform,
geom_end_transform])
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1):
distance = psub(p2, p1)
length = pnorm(distance)
direction = pnormalized(distance)
if direction is None:
# zero-length line
return
cone_length = length * size
cone_radius = cone_length / 3.0
# move the cone to the middle of the line
GL.glTranslatef((p1[0] + p2[0]) * position, (p1[1] + p2[1]) * position,
(p1[2] + p2[2]) * position)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = pcross(direction, (0, 0, -1))
if pnorm(cross) != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction[0]**2 + direction[1]**2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction[2] < 0:
angle = 180 - angle
GL.glRotatef(angle, cross[0], cross[1], cross[2])
elif direction[2] == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length * position)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
def draw_direction_cone(p1, p2, position=0.5, precision=12, size=0.1):
distance = psub(p2, p1)
length = pnorm(distance)
direction = pnormalized(distance)
if direction is None:
# zero-length line
return
cone_length = length * size
cone_radius = cone_length / 3.0
# move the cone to the middle of the line
GL.glTranslatef((p1[0] + p2[0]) * position,
(p1[1] + p2[1]) * position, (p1[2] + p2[2]) * position)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = pcross(direction, (0, 0, -1))
if pnorm(cross) != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction[0] ** 2 + direction[1] ** 2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction[2] < 0:
angle = 180 - angle
GL.glRotatef(angle, cross[0], cross[1], cross[2])
elif direction[2] == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length * position)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, precision, 1)
def draw_direction_cone(p1, p2):
distance = p2.sub(p1)
length = distance.norm
direction = distance.normalized()
if direction is None:
# zero-length line
return
cone_radius = length / 30
cone_length = length / 10
# move the cone to the middle of the line
GL.glTranslatef((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = direction.cross(Point(0, 0, -1))
if cross.norm != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction.x ** 2 + direction.y ** 2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction.z < 0:
angle = 180 - angle
GL.glRotatef(angle, cross.x, cross.y, cross.z)
elif direction.z == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length / 2)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, 12, 1)
def draw_direction_cone(p1, p2):
distance = p2.sub(p1)
length = distance.norm
direction = distance.normalized()
if direction is None:
# zero-length line
return
cone_radius = length / 30
cone_length = length / 10
# move the cone to the middle of the line
GL.glTranslatef((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2)
# rotate the cone according to the line direction
# The cross product is a good rotation axis.
cross = direction.cross(Point(0, 0, -1))
if cross.norm != 0:
# The line direction is not in line with the z axis.
try:
angle = math.asin(sqrt(direction.x ** 2 + direction.y ** 2))
except ValueError:
# invalid angle - just ignore this cone
return
# convert from radians to degree
angle = angle / math.pi * 180
if direction.z < 0:
angle = 180 - angle
GL.glRotatef(angle, cross.x, cross.y, cross.z)
elif direction.z == -1:
# The line goes down the z axis - turn it around.
GL.glRotatef(180, 1, 0, 0)
else:
# The line goes up the z axis - nothing to be done.
pass
# center the cone
GL.glTranslatef(0, 0, -cone_length / 2)
# draw the cone
GLUT.glutSolidCone(cone_radius, cone_length, 12, 1)
def dist_to_point(self, p):
return sqrt(self.dist_to_point_sq(p))
def pdist(a, b, axes=None):
return sqrt(pdist_sq(a, b, axes=axes))
def norm(self):
if self._norm is None:
self._norm = sqrt(self.normsq)
return self._norm
def zoom_in(self):
self.scale_distance(sqrt(0.5))
def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
if pdot(d, axis) == 0:
if pdot(direction, axis) == 0:
return (None, None, INFINITE)
plane = Plane(center, axis)
(p1, l) = plane.intersect_point(direction, edge.p1)
(p2, l) = plane.intersect_point(direction, edge.p2)
pc = Line(p1, p2).closest_point(center)
d_sq = pnormsq(psub(pc, center))
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = pdot(psub(p1, pc), d)
d2 = pdot(psub(p2, pc), d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = psub(p1, pmul(direction, l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = psub(p2, pmul(direction, l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = psub(pc, pmul(direction, l))
return (ccp, cp, -l)
n = pcross(d, direction)
if pnorm(n)== 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = pnormalized(n)
# take a plane through the base
plane = Plane(center, axis)
# intersect base with line
(lp, l) = plane.intersect_point(d, edge.p1)
if not lp:
return (None, None, INFINITE)
# intersection of 2 planes: lp + \lambda v
v = pcross(axis, n)
if pnorm(v) == 0:
return (None, None, INFINITE)
v = pnormalized(v)
# take plane through intersection line and parallel to axis
n2 = pcross(v, axis)
if pnorm(n2) == 0:
return (None, None, INFINITE)
n2 = pnormalized(n2)
# distance from center to this plane
dist = pdot(n2, center) - pdot(n2, lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if pdot(d, axis) < 0:
dist2 = -dist2
ccp = psub(center, psub(pmul(n2, dist), pmul(v, dist2)))
plane = Plane(edge.p1, pcross(pcross(d, direction), d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def zoom_out(self):
self.scale_distance(sqrt(2))
def zoom_in(self):
self.scale_distance(sqrt(0.5))
def pnorm(a):
return sqrt(pdot(a, a))
def dist_to_point(self, p):
return sqrt(self.dist_to_point_sq(p))
def pdist(a, b, axes=None):
return sqrt(pdist_sq(a, b, axes=axes))
def zoom_out(self):
self.scale_distance(sqrt(2))
def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
if d.dot(axis) == 0:
if direction.dot(axis) == 0:
return (None, None, INFINITE)
plane = Plane(center, axis)
(p1, l) = plane.intersect_point(direction, edge.p1)
(p2, l) = plane.intersect_point(direction, edge.p2)
pc = Line(p1, p2).closest_point(center)
d_sq = pc.sub(center).normsq
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = p1.sub(pc).dot(d)
d2 = p2.sub(pc).dot(d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = p1.sub(direction.mul(l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = p2.sub(direction.mul(l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = pc.sub(direction.mul(l))
return (ccp, cp, -l)
n = d.cross(direction)
if n.norm == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = n.normalized()
# take a plane through the base
plane = Plane(center, axis)
# intersect base with line
(lp, l) = plane.intersect_point(d, edge.p1)
if not lp:
return (None, None, INFINITE)
# intersection of 2 planes: lp + \lambda v
v = axis.cross(n)
if v.norm == 0:
return (None, None, INFINITE)
v = v.normalized()
# take plane through intersection line and parallel to axis
n2 = v.cross(axis)
if n2.norm == 0:
return (None, None, INFINITE)
n2 = n2.normalized()
# distance from center to this plane
dist = n2.dot(center) - n2.dot(lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if d.dot(axis) < 0:
dist2 = -dist2
ccp = center.sub(n2.mul(dist)).sub(v.mul(dist2))
plane = Plane(edge.p1, d.cross(direction).cross(d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
if d.dot(axis) == 0:
if direction.dot(axis) == 0:
return (None, None, INFINITE)
plane = Plane(center, axis)
(p1, l) = plane.intersect_point(direction, edge.p1)
(p2, l) = plane.intersect_point(direction, edge.p2)
pc = Line(p1, p2).closest_point(center)
d_sq = pc.sub(center).normsq
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = p1.sub(pc).dot(d)
d2 = p2.sub(pc).dot(d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = p1.sub(direction.mul(l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = p2.sub(direction.mul(l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = pc.sub(direction.mul(l))
return (ccp, cp, -l)
n = d.cross(direction)
if n.norm == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = n.normalized()
# take a plane through the base
plane = Plane(center, axis)
# intersect base with line
(lp, l) = plane.intersect_point(d, edge.p1)
if not lp:
return (None, None, INFINITE)
# intersection of 2 planes: lp + \lambda v
v = axis.cross(n)
if v.norm == 0:
return (None, None, INFINITE)
v = v.normalized()
# take plane through intersection line and parallel to axis
n2 = v.cross(axis)
if n2.norm == 0:
return (None, None, INFINITE)
n2 = n2.normalized()
# distance from center to this plane
dist = n2.dot(center) - n2.dot(lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if d.dot(axis) < 0:
dist2 = -dist2
ccp = center.sub(n2.mul(dist)).sub(v.mul(dist2))
plane = Plane(edge.p1, d.cross(direction).cross(d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def pnorm(a):
return sqrt(pdot(a,a))
def get_collision_waterline_of_triangle(model, cutter, up_vector, triangle, z):
# TODO: there are problems with "material allowance > 0"
plane = Plane(Point(0, 0, z), up_vector)
if triangle.minz >= z:
# no point of the triangle is below z
# try all edges
# Case (4)
proj_points = []
for p in triangle.get_points():
proj_p = plane.get_point_projection(p)
if not proj_p in proj_points:
proj_points.append(proj_p)
if len(proj_points) == 3:
edges = []
for index in range(3):
edge = Line(proj_points[index - 1], proj_points[index])
# the edge should be clockwise around the model
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
edge = Line(edge.p2, edge.p1)
edges.append((edge, proj_points[index - 2]))
outer_edges = []
for edge, other_point in edges:
# pick only edges, where the other point is on the right side
if other_point.sub(edge.p1).cross(edge.dir).dot(up_vector) > 0:
outer_edges.append(edge)
if len(outer_edges) == 0:
# the points seem to be an one line
# pick the longest edge
long_edge = edges[0][0]
for edge, other_point in edges[1:]:
if edge.len > long_edge.len:
long_edge = edge
outer_edges = [long_edge]
else:
edge = Line(proj_points[0], proj_points[1])
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
edge = Line(edge.p2, edge.p1)
outer_edges = [edge]
else:
# some parts of the triangle are above and some below the cutter level
# Cases (2a), (2b), (3a) and (3b)
points_above = [plane.get_point_projection(p)
for p in triangle.get_points() if p.z > z]
waterline = plane.intersect_triangle(triangle)
if waterline is None:
if len(points_above) == 0:
# the highest point of the triangle is at z
outer_edges = []
else:
if abs(triangle.minz - z) < epsilon:
# This is just an accuracy issue (see the
# "triangle.minz >= z" statement above).
outer_edges = []
elif not [p for p in triangle.get_points()
if p.z > z + epsilon]:
# same as above: fix for inaccurate floating calculations
outer_edges = []
else:
# this should not happen
raise ValueError(("Could not find a waterline, but " \
+ "there are points above z level (%f): " \
+ "%s / %s") % (z, triangle, points_above))
else:
# remove points that are not part of the waterline
points_above = [p for p in points_above
if (p != waterline.p1) and (p != waterline.p2)]
if len(points_above) == 0:
# part of case (2a)
outer_edges = [waterline]
elif len(points_above) == 1:
other_point = points_above[0]
dot = other_point.sub(waterline.p1).cross(waterline.dir).dot(
up_vector)
if dot > 0:
# Case (2b)
outer_edges = [waterline]
elif dot < 0:
# Case (3b)
edges = []
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
outer_edges = []
for edge in edges:
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
outer_edges.append(Line(edge.p2, edge.p1))
else:
outer_edges.append(edge)
else:
# the three points are on one line
# part of case (2a)
edges = []
edges.append(waterline)
edges.append(Line(waterline.p1, other_point))
edges.append(Line(waterline.p2, other_point))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
# two points above
other_point = points_above[0]
dot = other_point.sub(waterline.p1).cross(waterline.dir).dot(
up_vector)
if dot > 0:
# Case (2b)
# the other two points are on the right side
outer_edges = [waterline]
elif dot < 0:
# Case (3a)
edge = Line(points_above[0], points_above[1])
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
else:
edges = []
# pick the longest combination of two of these points
# part of case (2a)
# TODO: maybe we should use the waterline instead?
# (otherweise the line could be too long and thus
# connections to the adjacent waterlines are not discovered?
# Test this with an appropriate test model.)
points = [waterline.p1, waterline.p2] + points_above
for p1 in points:
for p2 in points:
if not p1 is p2:
edges.append(Line(p1, p2))
edges.sort(key=lambda x: x.len)
edge = edges[-1]
if edge.dir.cross(triangle.normal).dot(up_vector) < 0:
outer_edges = [Line(edge.p2, edge.p1)]
else:
outer_edges = [edge]
# calculate the maximum diagonal length within the model
x_dim = abs(model.maxx - model.minx)
y_dim = abs(model.maxy - model.miny)
z_dim = abs(model.maxz - model.minz)
max_length = sqrt(x_dim ** 2 + y_dim ** 2 + z_dim ** 2)
result = []
for edge in outer_edges:
direction = up_vector.cross(edge.dir).normalized()
if direction is None:
continue
direction = direction.mul(max_length)
edge_dir = edge.p2.sub(edge.p1)
# TODO: Adapt the number of potential starting positions to the length
# of the line. Don't use 0.0 and 1.0 - this could result in ambiguous
# collisions with triangles sharing these vertices.
for factor in (0.5, epsilon, 1.0 - epsilon, 0.25, 0.75):
start = edge.p1.add(edge_dir.mul(factor))
# We need to use the triangle collision algorithm here - because we
# need the point of collision in the triangle.
collisions = get_free_paths_triangles([model], cutter, start,
start.add(direction), return_triangles=True)
for index, coll in enumerate(collisions):
if (index % 2 == 0) and (not coll[1] is None) \
and (not coll[2] is None) \
and (coll[0].sub(start).dot(direction) > 0):
cl, hit_t, cp = coll
break
else:
log.debug("Failed to detect any collision: " \
+ "%s / %s -> %s" % (edge, start, direction))
continue
proj_cp = plane.get_point_projection(cp)
# e.g. the Spherical Cutter often does not collide exactly above
# the potential collision line.
# TODO: maybe an "is cp inside of the triangle" check would be good?
if (triangle is hit_t) or (edge.is_point_inside(proj_cp)):
result.append((cl, edge))
# continue with the next outer_edge
break
# Don't check triangles again that are completely above the z level and
# did not return any collisions.
if (len(result) == 0) and (triangle.minz > z):
# None indicates that the triangle needs no further evaluation
return None
return result
def norm(self):
if self._norm is None:
self._norm = sqrt(self.normsq)
return self._norm
