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