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 rotate_camera_by_screen(self, start_x, start_y, end_x, end_y):
factors_x, factors_y = self._get_axes_vectors()
width, height = self._get_screen_dimensions()
# calculate rotation factors - based on the distance to the center
# (between -1 and 1)
rot_x_factor = (2.0 * start_x) / width - 1
rot_y_factor = (2.0 * start_y) / height - 1
# calculate rotation angles (between -90 and +90 degrees)
xdiff = end_x - start_x
ydiff = end_y - start_y
# compensate inverse rotation left/right side (around x axis) and
# top/bottom (around y axis)
if rot_x_factor < 0:
ydiff = -ydiff
if rot_y_factor > 0:
xdiff = -xdiff
rot_x_angle = rot_x_factor * math.pi * ydiff / height
rot_y_angle = rot_y_factor * math.pi * xdiff / width
# rotate around the "up" vector with the y-axis rotation
original_distance = self.view["distance"]
original_up = self.view["up"]
y_rot_matrix = Matrix.get_rotation_matrix_axis_angle(
factors_y, rot_y_angle)
new_distance = Matrix.multiply_vector_matrix(original_distance,
y_rot_matrix)
new_up = Matrix.multiply_vector_matrix(original_up, y_rot_matrix)
# rotate around the cross vector with the x-axis rotation
x_rot_matrix = Matrix.get_rotation_matrix_axis_angle(
factors_x, rot_x_angle)
new_distance = Matrix.multiply_vector_matrix(new_distance,
x_rot_matrix)
new_up = Matrix.multiply_vector_matrix(new_up, x_rot_matrix)
self.view["distance"] = new_distance
self.view["up"] = new_up
def rotate_camera_by_screen(self, start_x, start_y, end_x, end_y):
factors_x, factors_y = self._get_axes_vectors()
width, height = self._get_screen_dimensions()
# calculate rotation factors - based on the distance to the center
# (between -1 and 1)
rot_x_factor = (2.0 * start_x) / width - 1
rot_y_factor = (2.0 * start_y) / height - 1
# calculate rotation angles (between -90 and +90 degrees)
xdiff = end_x - start_x
ydiff = end_y - start_y
# compensate inverse rotation left/right side (around x axis) and
# top/bottom (around y axis)
if rot_x_factor < 0:
ydiff = -ydiff
if rot_y_factor > 0:
xdiff = -xdiff
rot_x_angle = rot_x_factor * math.pi * ydiff / height
rot_y_angle = rot_y_factor * math.pi * xdiff / width
# rotate around the "up" vector with the y-axis rotation
original_distance = self.view["distance"]
original_up = self.view["up"]
y_rot_matrix = Matrix.get_rotation_matrix_axis_angle(factors_y,
rot_y_angle)
new_distance = Matrix.multiply_vector_matrix(original_distance,
y_rot_matrix)
new_up = Matrix.multiply_vector_matrix(original_up, y_rot_matrix)
# rotate around the cross vector with the x-axis rotation
x_rot_matrix = Matrix.get_rotation_matrix_axis_angle(factors_x,
rot_x_angle)
new_distance = Matrix.multiply_vector_matrix(new_distance, x_rot_matrix)
new_up = Matrix.multiply_vector_matrix(new_up, x_rot_matrix)
self.view["distance"] = new_distance
self.view["up"] = new_up
def get_bezier_lines(points_with_bulge, segments=32):
# TODO: add a recursive algorithm for more than two points
if len(points_with_bulge) != 2:
return []
else:
result_points = []
p1, bulge1 = points_with_bulge[0]
p2, bulge2 = points_with_bulge[1]
if not bulge1 and not bulge2:
# straight line
return [Line(p1, p2)]
straight_dir = pnormalized(psub(p2, p1))
bulge1 = math.atan(bulge1)
rot_matrix = Matrix.get_rotation_matrix_axis_angle((0, 0, 1),
-2 * bulge1,
use_radians=True)
dir1_mat = Matrix.multiply_vector_matrix(
(straight_dir[0], straight_dir[1], straight_dir[2]), rot_matrix)
dir1 = (dir1_mat[0], dir1_mat[1], dir1_mat[2], 'v')
if bulge2 is None:
bulge2 = bulge1
else:
bulge2 = math.atan(bulge2)
rot_matrix = Matrix.get_rotation_matrix_axis_angle((0, 0, 1),
2 * bulge2,
use_radians=True)
dir2_mat = Matrix.multiply_vector_matrix(
(straight_dir[0], straight_dir[1], straight_dir[2]), rot_matrix)
dir2 = (dir2_mat[0], dir2_mat[1], dir2_mat[2], 'v')
# interpretation of bulge1 and bulge2:
# /// taken from http://paulbourke.net/dataformats/dxf/dxf10.html ///
# The bulge is the tangent of 1/4 the included angle for an arc
# segment, made negative if the arc goes clockwise from the start
# point to the end point; a bulge of 0 indicates a straight segment,
# and a bulge of 1 is a semicircle.
alpha = 2 * (abs(bulge1) + abs(bulge2))
dist = pdist(p2, p1)
# calculate the radius of the circumcircle - avoiding divide-by-zero
if (abs(alpha) < epsilon) or (abs(math.pi - alpha) < epsilon):
radius = dist / 2.0
else:
# see http://en.wikipedia.org/wiki/Law_of_sines
radius = abs(dist / math.sin(alpha / 2.0)) / 2.0
# The calculation of "factor" is based on random guessing - but it
# seems to work well.
factor = 4 * radius * math.tan(alpha / 4.0)
dir1 = pmul(dir1, factor)
dir2 = pmul(dir2, factor)
for index in range(segments + 1):
# t: 0..1
t = float(index) / segments
# see: http://en.wikipedia.org/wiki/Cubic_Hermite_spline
p = padd(
pmul(p1, 2 * t**3 - 3 * t**2 + 1),
padd(
pmul(dir1, t**3 - 2 * t**2 + t),
padd(pmul(p2, -2 * t**3 + 3 * t**2),
pmul(dir2, t**3 - t**2))))
result_points.append(p)
# create lines
result = []
for index in range(len(result_points) - 1):
result.append(Line(result_points[index], result_points[index + 1]))
return result
