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 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 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])
