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 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 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 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 pnorm(a): return sqrt(pdot(a, a))
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