def project_to_ground(self, cx, cy): "Converts camera coordinates to a ground point in the base frame." # Formula taken from Tekkotsu's projectToGround method camera_res = (320, 240) half_camera_max = max(*camera_res) / 2 config = self.robot.camera.config # Convert to generalized coordinates in range [-1, 1] gx = (cx - config.center.x) / half_camera_max gy = (cy - config.center.y) / half_camera_max #tekkotsu_focal_length_x = camera_res[0]/camera_max / tan(config.fov_x.radians/2) #tekkotsu_focal_length_y = camera_res[1]/camera_max / tan(config.fov_y.radians/2) # Generate a ray in the camera frame rx = gx / (config.focal_length.x / half_camera_max) ry = gy / (config.focal_length.y / half_camera_max) ray = point(rx, ry, 1) cam_to_base = self.robot.kine.joint_to_base('camera') offset = translation_part(cam_to_base) rot_ray = rotation_part(cam_to_base).dot(ray) dist = -offset[2, 0] align = rot_ray[2, 0] if abs(align) > 1e-5: s = dist / align hit = point(rot_ray[0, 0] * s, rot_ray[1, 0] * s, rot_ray[2, 0] * s) + offset elif align * dist < 0: hit = point(-rot_ray[0, 0], -rot_ray[1, 0], -rot_ray[2, 0], abs(align)) else: hit = point(rot_ray[0, 0], rot_ray[1, 0], rot_ray[2, 0], abs(align)) return hit
def setscan(pointList): circles = [] for center in pointList: for boundary in pointList: if center == boundary: continue centerPoint = geometry.point(center[0][0], center[0][1], center[1]) circles.append( geometry.circle(centerPoint, centerPoint.euclideanDistance(boundary[0]))) for enumurateCircle in circles: B = totalCases * (enumurateCircle.area() / UsArea) c = 0 for p in pointList: ConsideredPoint = geometry.point(p[0][0], p[0][1], p[1]) if enumurateCircle.LieInside(ConsideredPoint): c += ConsideredPoint.a logLRz = 0 #print(totalCases,B,c) if totalCases == c: logLRz = -math.inf elif c > B: logLRz += c * math.log(c / B) logLRz += (totalCases - c) * math.log( (totalCases - c) / (totalCases - B)) enumurateCircle.logLRz = logLRz return circles
def drawScene(): """ Issue GL calls to draw the scene. """ # Clear the rendering information. glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) # Clear the transformation stack. glMatrixMode(GL_MODELVIEW) glLoadIdentity() # Transform the objects drawn below by a rotation. trackball.glRotate() # Draw a floating "pixel." glColor(1.0, 1.0, 1.0) glPointSize(20) # glBegin(GL_POINTS) point(0.0, 0.0, 2.0).glVertex3() glEnd() # Draw the object, drawObject() # Render the scene. glFlush()
def update_aruco_landmarks(self): try: seen_marker_objects = self.robot.world.aruco.seen_marker_objects.copy( ) except: return aruco_parent = self.robot.world.aruco for (key, value) in seen_marker_objects.items(): marker_id = value.id_string wmobject = self.objects.get(marker_id, None) if wmobject is None: # TODO: wait to see marker several times before adding. wmobject = ArucoMarkerObj( aruco_parent, key) # coordinates will be filled in below self.objects[marker_id] = wmobject landmark_spec = None else: landmark_spec = self.robot.world.particle_filter.sensor_model.landmarks.get( marker_id, None) wmobject.pose_confidence = +1 if isinstance(landmark_spec, tuple): # Particle filter is tracking this marker wmobject.x = landmark_spec[0][0][0] wmobject.y = landmark_spec[0][1][0] wmobject.theta = landmark_spec[1] elevation = atan2(value.camera_coords[1], value.camera_coords[2]) cam_pos = geometry.point( 0, value.camera_distance * sin(elevation), value.camera_distance * cos(elevation)) base_pos = self.robot.kine.joint_to_base('camera').dot(cam_pos) wmobject.z = base_pos[2, 0] wmobject.elevation = elevation wmobject.cam_pos = cam_pos wmobject.base_pos = base_pos elif isinstance( landmark_spec, Pose): # Hard-coded landmark pose for laboratory exercises wmobject.x = landmark_spec.position.x wmobject.y = landmark_spec.position.y wmobject.theta = landmark_spec.rotation.angle_z.radians wmobject.is_fixed = True else: # Non-landmark: convert aruco sensor values to pf coordinates and update elevation = atan2(value.camera_coords[1], value.camera_coords[2]) cam_pos = geometry.point( 0, value.camera_distance * sin(elevation), value.camera_distance * cos(elevation)) base_pos = self.robot.kine.joint_to_base('camera').dot(cam_pos) wmobject.x = base_pos[0, 0] wmobject.y = base_pos[1, 0] wmobject.z = base_pos[2, 0] wmobject.theta = wrap_angle( self.robot.world.particle_filter.pose[2] + value.euler_rotation[1] * (pi / 180) + pi) wmobject.elevation = elevation wmobject.cam_pos = cam_pos wmobject.base_pos = base_pos
def __init__(self, rec, x_mesh_num, y_mesh_num): rec = tuple(rec) assert isinstance(rec, tuple) assert all([isinstance(p, gm.point) for p in rec]) assert all(rec[0].np_params < rec[1].np_params) assert all( [isinstance(x, int) and x > 2 for x in [x_mesh_num, y_mesh_num]]) super().__init__() self.rec = rec self.x_mesh_num = x_mesh_num self.y_mesh_num = y_mesh_num self.a, self.b = [x.params for x in self.rec] self.x_mesh = (self.b[0] - self.a[0]) / self.x_mesh_num self.y_mesh = (self.b[1] - self.a[1]) / self.y_mesh_num # skeleton_points self.skeleton_points = [] for i in range(self.x_mesh_num + 1): skl = [] self.skeleton_points.append(skl) for j in range(self.y_mesh_num + 1): p = gm.point([i * self.x_mesh, j * self.x_mesh]) self.points.append(p) self.point_boundary[p] = [] self.point_triangles[p] = [] skl.append(p) # triangles and mid points self.mid_points = [] for i in range(self.x_mesh_num): mid = [] self.mid_points.append(mid) for j in range(self.y_mesh_num): p1, p2, p3, p4 = self.skeleton_points[i][j], self.skeleton_points[i + 1][j],\ self.skeleton_points[i][j + 1], self.skeleton_points[i + 1][j + 1] params = (p1.np_params + p4.np_params) / 2 q = gm.point(list(params)) mid.append(q) self.points.append(q) self.point_triangles[q] = [] self.point_boundary[q] = [] for l in [[p1, p2, q], [p1, p3, q], [p3, p4, q], [p2, p4, q]]: t = gm.triangle(l) self.triangles.append(t) for p in l: self.point_triangles[p].append(t) # boundry row1 = self.skeleton_points[0] row2 = self.skeleton_points[self.y_mesh_num][::-1] col1 = [row[0] for row in self.skeleton_points[::-1]] col2 = [row[self.x_mesh_num] for row in self.skeleton_points] boundary = [row1, col2, row2, col1] for row in boundary: for i in range(len(row) - 1): l = [row[i], row[i + 1]] seg = gm.segment(l) self.boundary.append(seg) for p in l: self.point_boundary[p].append(seg) # dictionary self.point_dictionary = {p: i for i, p in enumerate(self.points)}
def getSpeedSlope(self, shapeFile, subTrack, slopeSpeedOutFileHdl, paramDict, speedSlopeOutFileHdl): # one record per point # get distance # get duration # get speed # get altitudes # get slope #write speed and slope pointList = self.pointLists[subTrack] if speedSlopeOutFileHdl: slopeSpeedOutFileHdl.write("ShapeFile" + delim + "SubTrack" + delim + "Distance" + delim + "FromID" + delim + "ToID" + delim + "Speed" + delim + "Slope" + delim + "\n") pp0 = point(0,0) slopeList = [] speedList = [] for iP in range(len(pointList)): p1 = pointList[iP].obj.GetGeometryRef().GetPoint(0) pp1 = point(p1[0], p1[1]) distance = 0.0 offset = 1 if iP > 0: while distance < slopeSpeedResolution and iP+offset < len(pointList): ##print iP+offset, len(pointList) p2 = pointList[iP+offset].obj.GetGeometryRef().GetPoint(0) pp2 = point(p2[0], p2[1]) p3 = pointList[iP+offset-1].obj.GetGeometryRef().GetPoint(0) pp3 = point(p3[0], p3[1]) ## Collect slopes on the way (as a list). Enable average slopes by the end. distance += dist(pp3, pp2) startId = pointList[iP].obj.GetField(pointIdAttributeName) endId = pointList[iP+offset].obj.GetField(pointIdAttributeName) startTime = pointList[iP].dateTime endTime = pointList[iP+offset].dateTime offset += 1 deltaTimeHours = (endTime - startTime).seconds / 3600.00 rasterVal1 = paramDict["Altitude"].getCellValueAtGeolocation(pp2.x, pp2.y) rasterVal0 = paramDict["Altitude"].getCellValueAtGeolocation(pp0.x, pp0.y) deltaAltitude = rasterVal1 - rasterVal0 if distance != 0: ## Calc slopePCT as the average oof all segments slope slopePct = (deltaAltitude * 100.00) / distance else: slopePct = 0 speed = (distance / 1000) / deltaTimeHours if slopePct > minSlope and slopePct < maxSlope and speed > minSpeed and speed < maxSpeed: slopeSpeedOutFileHdl.write(shapeFile + delim + str(subTrack) + delim + str(distance) + delim + str(startId) + delim + str(endId) + delim + str(speed) + delim + str(slopePct) + "\n") slopeList.append(slopePct) speedList.append(speed) p0 = p1 pp0 = point(p0[0], p0[1]) ##print len(slopeList), len(speedList), len([slopeList, speedList]) return [slopeList, speedList]
def getSamples(self, subIndex, angleWidth, length, numPoints, targetFileHdl, rasterDataDict={}, header=False): ##print length pointList = self.pointLists[subIndex] lastPoint = pointList[-1].obj ##import pdb;pdb.set_trace() p3 = lastPoint.GetGeometryRef().GetPoint(0) paramListStr = "" orgLength = length for key in rasterDataDict.keys(): paramListStr = paramListStr + delim + key if header: targetFileHdl.write("FileName" + delim + "SubTrack" + delim + "Id" + delim + "Choice" + delim + "Distance" + delim + "X" + delim + "Y" + delim + "AngPrev" + delim + "AngLast" + paramListStr + "\n") for i in range(len(pointList)): length = orgLength if i == 0: presentPoint = pointList[0].obj if i == 1: previousPoint = presentPoint presentPoint = pointList[1].obj if i > 1: for ii in range(len(pointList)): if ii >= i: pp1 = presentPoint.GetGeometryRef().GetPoint(0) pp1 = point(pp1[0], pp1[1]) pp2 = pointList[ii].obj.GetGeometryRef().GetPoint(0) pp2 = point(pp2[0], pp2[1]) akkuLength = dist(pp1, pp2) ##import pdb;pdb.set_trace() ##print "Length", length, "akkuLength", akkuLength if akkuLength >= length: samplePoint = pointList[ii].obj ##pp2 = nextPoint.GetGeometryRef().GetPoint(0) break else: ##nextPoint = pointList[ii - 1].obj pass length = akkuLength nextPoint = pointList[i].obj id = presentPoint.GetField(pointIdAttributeName) p0 = previousPoint.GetGeometryRef().GetPoint(0) p1 = presentPoint.GetGeometryRef().GetPoint(0) p2 = samplePoint.GetGeometryRef().GetPoint(0) samples = samplePoints(point(p1[0], p1[1]), point(p2[0], p2[1]), angleWidth, length, numPoints, point(p0[0], p0[1]), point(p3[0], p3[1])) for p in samples: ## Here raster values are collected. paramValueStr = "" for key in rasterDataDict.keys(): paramValueStr = paramValueStr + delim + str(rasterDataDict[key].getCellValueAtGeolocation(p.x, p.y)) targetFileHdl.write(self.fileName + delim + str(subIndex) + delim + str(id) + delim + str(p.selected) + delim + str("%.2f" % p.length) + delim + str(p.x) + delim + str(p.y) + delim + str("%.4f" % p.angleToLast) + delim + str("%.4f" % p.angleFromPrevious) + paramValueStr + "\n") previousPoint = presentPoint presentPoint = nextPoint
def motion(x, y): global trackball, xStart, yStart xNow = (x - width / 2) * scale yNow = (height / 2 - y) * scale change = point(xNow, yNow, 0.0) - point(xStart, yStart, 0.0) axis = vector(-change.dy, change.dx, 0.0) sin_angle = change.norm() / radius sin_angle = max(min(sin_angle, 1.0), -1.0) # clip angle = asin(sin_angle) trackball = quat.for_rotation(angle, axis) * trackball xStart, yStart = xNow, yNow glutPostRedisplay()
def getSubTrackLength(self, subTrack): pointList = self.pointLists[subTrack] totalLength = 0 p0 = point(0,0) for iP in range(len(pointList)): p1 = pointList[iP].obj.GetGeometryRef().GetPoint(0) p1 = point(p1[0], p1[1]) if iP > 0: totalLength += dist(p0, p1) else: pFirst = p1 p0 = p1 return totalLength, dist(pFirst, p1)
def motion(x, y): global trackball, xStart, yStart xNow = (x - width/2) * scale yNow = (height/2 - y) * scale change = point(xNow,yNow,0.0) - point(xStart,yStart,0.0) axis = vector(-change.dy,change.dx,0.0) sin_angle = change.norm()/radius sin_angle = max(min(sin_angle,1.0),-1.0) # clip angle = asin(sin_angle) trackball = quat.for_rotation(angle,axis) * trackball xStart,yStart = xNow, yNow glutPostRedisplay()
def make_ritchey_chretien(focal_length, D, b, aperture_radius): """Ritchey-Chrétien Args: focal_length: focal length D: distance between primary and secondary (along axis) b: backfocus from primary to focal plane aperture_radius: aperture radius Returns: The resulting Instrument Notation follows https://en.wikipedia.org/wiki/Ritchey%E2%80%93Chr%C3%A9tien_telescope """ debug = True F = focal_length B = D + b R1 = -(2 * D * F) / (F - B) R2 = -(2 * D * B) / (F - B - D) f1 = np.abs(R1) / 2 f2 = np.abs(R2) / 2 M = F / f1 M_alt = (F - B) / D print(f"M={M}, M_alt={M_alt}") assert np.isclose(M, M_alt) K1 = -1 - (2 / M**3) * (B / D) K2 = -1 - (2 / (M - 1)**3) * (M * (2 * M - 1) + B / D) print( f"primary: trying to make hyperboloid with radius {-R1} conic constant {K1} at z=0" ) primary = make_conic(-R1, K1, 0) print(f"primary: {primary}") print( f"secondary: trying to make hyperboloid with radius {-R2} conic constant {K2} at z={D}" ) secondary = make_conic(-R2, K2, D, reverse_normal=True) print(f"secondary: {secondary}") aperture0 = CircularAperture(point(0, 0, D), vector(0, 0, -aperture_radius)) # The rule we used for the parabolic should still be approximately correct here. z_reflector_edge = aperture_radius**2 / (4 * f1) aperture1 = CircularAperture(point(0, 0, z_reflector_edge), vector(0, 0, -aperture_radius)) #source = standard_source(focal_length, aperture_radius) source = standard_source(D, aperture_radius) elements = Compound([aperture0, aperture1, primary, secondary]) sensor = PlanarSensor.of_q_x_y(point(0, 0, -b), -ii, -jj) return Instrument(source, elements, sensor)
def init(): global ctl_pts, m_pts #set the control points for a triangle c0 = point((radius-1),0,0) c1 = point((1-radius),0,0) c2 = point(0,(radius-1),0) ctl_pts = [c0, c1, c2] #subdivide into three curves m = [] = array of midpoints m0 = c0.plus(c1.minus(c0).scale(.5)) m1 = c1.plus(c2.minus(c1).scale(.5)) m2 = c2.plus(c0.minus(c2).scale(.5)) m_pts = [m0,m1,m2]
def __init__(self): self.triangles = [] # A list of triangle objects. self.verts = [] # A list of vertices (vertex objects) self.edges = {} # a dictionary of edge objects. self.shadows = [] # a list of triangle objects, that will # be shadows. For testing (should be done in hardware later) self.radius = 0.0 self.vindex = 0 # Current max vertex index self.tindex = 0 # Current max triangle index self.sindex = 0 # Current shadow triangle index self.p1 = point(10.0, -0.001, 10.0) # point on floor self.p2 = point(-10.0, -0.001, -10.0) # another point on the floor self.p3 = point(-10.0, -0.001, 10.0) # third point on floor
def make_simple_refractor(focal_length, aperture_radius, d): """Refractor with single lens d is thickness of lens """ # TODO: Check whether the value of d gives us a lens consistent with the aperture. source = standard_source(d, aperture_radius) # We'll think of the aperture as just slightly in front of the lens. aperture = CircularAperture(point(0, 0, 0.75 * d), vector(0, 0, -aperture_radius)) sensor = PlanarSensor.of_q_x_y(point(0, 0, -focal_length), -ii, -jj) z_offset = 0. lens = make_lens_basic(focal_length, d, z_offset) elements = Compound([aperture, lens]) return Instrument(source, elements, sensor)
def loop(self): done = False while not done: for event in pygame.event.get(): if event.type == pygame.QUIT: return self.mouse_position=self.getWorldCoords(point(*pygame.mouse.get_pos())) self.screen.fill((255,255,255)) for bicycle in self.bicycles: screenCoords=self.getScreenCoords(bicycle.position) pygame.draw.ellipse(self.screen,(255,0,0),pygame.Rect(screenCoords.x-5,screenCoords.y-7,15,15)) velocScreenCoords=self.getScreenCoords(bicycle.position.sum(bicycle.velocity)) pygame.draw.line(self.screen,(0,0,255),screenCoords.asTuple(),velocScreenCoords.asTuple(),2) if len(bicycle.path)>=2: for i in range(len(bicycle.path)-1): #point 1 screen coords pt1sc=self.getScreenCoords(bicycle.path[i]) pt2sc=self.getScreenCoords(bicycle.path[i+1]) pygame.draw.line(self.screen,(0,255,0),pt1sc.asTuple(),pt2sc.asTuple(),1) for bicycle in self.bicycles: a,d=bicycle.steer() opponents=[] for test_bicycle in self.bicycles: if test_bicycle is not bicycle: opponents.append(test_bicycle) bicycle.update_path(opponents) bicycle.update(a,d) pygame.display.flip() clock.tick(1/self.timeStep)
def read_obj(self,filename): vertex_id = 1 obj_file = open(filename,'r') for line in obj_file: parts = line[:-1].split() if len(parts) > 0: # Read a vertex description line. if parts[0] == 'v': x = float(parts[1]) y = float(parts[2]) z = float(parts[3]) P = point(x,y,z) self.make_vertex(P,vertex_id) vertex_id += 1 # Read a face/fan description line. elif parts[0] == 'f': #### SUBTRACTS 1 FROM THE .OBJ INDEX!!! (.OBJ starts at 1) #### vi_fan = [int(p.split('/')[0]) for p in parts[1:]] vi1 = vi_fan[0] # add the faces of the fan for i in range(1,len(vi_fan)-1): vi2 = vi_fan[i] vi3 = vi_fan[i+1] f = self.make_face(vi1,vi2,vi3) # rescale and center the points self.rebox() # fix the vertices self.fix_edges() obj_file.close()
def make_gregory_maksutov(aperture_radius, R1, R2, R3, d, ell, b, lens_material=None): """Gregory-Maksutov The sign conventions are such that, typically, all parameters below are positive. In particular, for the radii of curvature, positive means concave towards positive z-axis. R1 - radius of curvature of front of lens R2 - radius of curvature of back of lens (which is also secondary reflector) R3 - radius of curvature of primary reflector d - thickness of lens on axis ell - distance between primary and secondary reflector b - backfocus from primary to sensor lens_material - self explanatory; BK7 by default """ if aperture_radius > max(R1, R2, R3): raise ValueError( "aperture radius must not exceed any radius of curvature") # See https://www.cfht.hawaii.edu/~baril/Maksutov/Maksutov.html # We'll have primary reflector intersect z-axis at z=0. I'm not sure if that's # the convention we want. lens_z_offset = d / 2 + ell lens = make_lens(-R1, -R2, d, lens_z_offset, material=lens_material) # make primary and secondary... primary = make_conic(R3, 0., 0.) secondary = make_conic(R2, 0., ell) # How much farther forward is the rim of the lens from where its front meets # z-axis? front_depth = R1 - np.sqrt(R1**2 - aperture_radius**2) # TODO: also include an aperture at primary reflector? front_z_coordinate = ell + d + front_depth aperture0 = CircularAperture(point(0, 0, front_z_coordinate), vector(0, 0, -aperture_radius)) source = standard_source(front_z_coordinate, aperture_radius) elements = Compound([aperture0, lens, primary, secondary]) sensor = PlanarSensor.of_q_x_y(point(0, 0, -b), -ii, -jj) return Instrument(source, elements, sensor)
def add_anchor(self,ID,loc,IP=None): try: gx.AnchorDic[ID] print str(ID)+':Anchor with same ID already exists' return except KeyError: a=gx.Anchor(ID,gm.point(loc),IP=IP) gx.AnchorDic[ID]=a return a
def screenToWorldCoords(x, y): xnew = (2.0*x/width) - 1.0 ynew = 1.0 - (2.0*y/height) ctw = [xnew, ynew, 0.0, 0.0] for i in range(len(iproj)): ctw[i] = iproj[i][i]*ctw[i] return point(ctw[0], ctw[1], ctw[2])
def add_anchor(self, ID, loc): try: self.AnchorDic[ID] print str(ID) + ':Anchor with same ID already exists' return except KeyError: a = gx.Anchor(ID, gm.point(loc)) self.AnchorDic[ID] = a return a
def __init__(self, x,y,z,i): self.index = i # index in list. self.loc = point(x,y,z) self.adj_tris = [] # indices of triangles that are adjacent to this # triangle #self.color = [0.5,0.45,0.57] # default color - a nice stony white. self.color = [1.0, 0.0, 1.0] # bright purple!!!! self.normal = None self.subdivided = False # flag whether this edge has been subdivided yet
def add_anchor(self, ID, loc): try: self.AnchorDic[ID] self.log.error(str(ID)+':Anchor with same ID already exists') return except KeyError: a = Anchor(ID, geometry.point(loc)) self.AnchorDic[ID] = a return a
def genSphere(smooth): smoothness = smooth radius = 2 numPoints = 2 * pi / smoothness points = [] facets = [] #generate the points of the sphere for i in range(0, smoothness): #latitude (goes from (pi/2) to (-pi/2)) theta = ((i / 2) * numPoints - pi) for j in range(0, smoothness): #longitude (goes from -pi to pi) phi = j * numPoints # convert spherical coordinates into R^3 x = radius * sin(theta) * cos(phi) y = radius * sin(theta) * sin(phi) z = radius * cos(theta) points.append(point(x, y, z)) #generate the facets of the sphere #rows for row in range(0, smoothness): for p in range(0, smoothness): k = (p + 1) % (smoothness) p1 = smoothness * row + p p2 = smoothness * row + k p3 = smoothness * (row - 1) + k p4 = smoothness * (row - 1) + p #top facets # / \ # --- #account for first row # if (row+2) is smoothness: # p2 = smoothness*row # if (row-1) is 0: # p4 = smoothness*(row-1) if row is not 0: facets.append([p4, p3, p2]) #bottom facets # --- # \ / #account for last row # if (row-1) is 0: # p4 = smoothness*(row-1) facets.append([p1, p2, p4]) return points, facets
def make_conic(R, K, z_offset, material=None, reverse_normal=False): """ See https://en.wikipedia.org/wiki/Conic_constant r^2 - 2Rz + (K+1)z^2 = 0 Be careful about the sign convention for radius of curvature. We follow the convention in https://en.wikipedia.org/wiki/Conic_constant but this is opposite the convention in https://en.wikipedia.org/wiki/Lens#Lensmaker's_equation . Args: R: radius of curvature; use R > 0 for concave "up" (direction of positive z-axis) while R < 0 is concave "down" K: conic constant; should be < -1 for hyperboloids, -1 for paraboloids, > -1 for ellipses (including 0 for spheres). The relationship with eccentricity e is K = -e^2 (when K <= 0). z_offset: z-coordinate where the surface intersects z-axis material: mostly self explanatory; None means reflector reverse_normal: If true, surface points in direction of negative z-axis rather than positive z-axis """ M = np.diag([1, 1, (K + 1), 0]) M[2, 3] = -R M[3, 2] = M[2, 3] # For either sign of R, we want the convention that gradient points up at origin. # That gradient is (0,0,-R). # When R < 0, we already have that. # For R > 0, we need to negate M to get that. if R > 0: M *= -1 if reverse_normal: M *= -1 quad = Quadric(M) geometry = quad.untransform(translation3f(0, 0, -z_offset)) if R > 0: # We want to keep the top sheet. # TODO: Let clip_z be halfway between the two foci. clip_z = z_offset - 1e-6 clip = Plane(make_bound_vector(point(0, 0, clip_z), vector(0, 0, -1))) else: clip_z = z_offset + 1e-6 clip = Plane(make_bound_vector(point(0, 0, clip_z), vector(0, 0, 1))) return SubElement(geometry, clip, material=material)
def rebox(self): max_dims = point(sys.float_info.min, sys.float_info.min, sys.float_info.min) min_dims = point(sys.float_info.max, sys.float_info.max, sys.float_info.max) for V in self.vertex: max_dims = max_dims.max(V.position) min_dims = min_dims.min(V.position) span = max_dims - min_dims center = point((min_dims.x + max_dims.x)/2.0, min_dims.y, (min_dims.z + max_dims.z)/2.0) scale = 1.4/abs(max_dims - center) for V in self.vertex: V.position = ORIGIN + scale * (V.position-center)
def lse(cA, mode='2D', cons=True): l = len(cA) r = [w.r for w in cA] c = [w.c for w in cA] S = sum(r) W = [(S - w) / ((l - 1) * S) for w in r] p0 = gx.point(0, 0, 0) # Initialized point for i in range(l): p0 = p0 + W[i] * c[i] if mode == '2D' or mode == 'Earth1': x0 = num.array([p0.x, p0.y]) elif mode == '3D': x0 = num.array([p0.x, p0.y, p0.z]) else: raise cornerCases('Mode not supported:' + mode) if mode == 'Earth1': fg1 = 1 else: fg1 = 0 if cons: print('GC-LSE geolocating...') if not is_disjoint(cA, fg=fg1): cL = [] for q in range(l): def ff(x, q=q): return r[q] - Norm(x, c[q].std(), mode=mode) cL.append(ff) res = fmin_cobyla(sum_error, x0, cL, args=(c, r, mode), consargs=(), rhoend=1e-5) ans = res else: raise cornerCases('Disjoint') else: print('LSE Geolocating...') res = minimize(sum_error, x0, args=(c, r, mode), method='BFGS') ans = res.x return gx.point(ans)
def drawScene(): """ Issue GL calls to draw the scene. """ # Clear the rendering information. glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) # Clear the transformation stack. glMatrixMode(GL_MODELVIEW) glLoadIdentity() # # Transform the objects drawn below by a rotation. trackball.glRotate() # Draw the light source "pixel" glColor(1.0,1.0,1.0) glPointSize(20) # glBegin(GL_POINTS) point(0.0,0.0, 4.0).glVertex3() glEnd() # Draw all the triangular facets. glBegin(GL_TRIANGLES) for f in facets: glColor4f(f.material.red, f.material.green, f.material.blue, f.material.alpha) # glColor4f(0.0, 0.0, 0.0, 0.0) glVertex3fv(f.vertices[0].point.components()) glVertex3fv(f.vertices[1].point.components()) glVertex3fv(f.vertices[2].point.components()) glEnd() for f in facets: glBegin(GL_LINES) for hedge in f.edges: glColor4f(.8, .0, .2, 1.0) dest = hedge.next.vsource.point glVertex3fv(hedge.vsource.point.components()) glVertex3fv(dest.components()) glEnd() # Render the scene. glFlush()
def rebox(cls): max_dims = point(sys.float_info.min, sys.float_info.min, sys.float_info.min) min_dims = point(sys.float_info.max, sys.float_info.max, sys.float_info.max) for V in vertex.all_instances(): max_dims = max_dims.max(V.position) min_dims = min_dims.min(V.position) origin = point((min_dims.x + max_dims.x)/2.0, (min_dims.y + max_dims.y)/2.0, (min_dims.z + max_dims.z)/2.0) delta = max_dims - min_dims biggest = max(delta.dx,max(delta.dy,delta.dz)) scale = sqrt(2.0)/biggest for V in vertex.all_instances(): V.position = ORIGIN + scale * (V.position-origin)
def projectShadows(self): """ Projects each triangle onto the FLOOR to create a shadow. Since the position of the floor is currently fixed, this isn't dynamic (it just assumes the floor is a plane containing the points set in compile()) Also keeps the light position static for now. """ p1 = self.p1 p2 = self.p2 p3 = self.p3 p1v = p1.minus(p2) p2v = p2.minus(p3) # these are two vectors on the plane l = point(1.0, 1.0, 0.0) # position of the light nv = p1v.cross(p2v) # plane normal vector nv = nv.unit() # First, we need to compute a vector that is normal to the plane # and intersects with l. This is computed by adding two vectors on the # plane, and seeing where the intersect with a l + d*nv. lp = l.minus(p1) # create matrices to solve a = array([[p2v.dx, p1v.dx, nv.dx], [p2v.dy, p1v.dy, nv.dy], [p2v.dz, p1v.dz, nv.dz]]) b = array([[lp.dx],[lp.dy], [lp.dz]]) solution = solve(a,b) d = solution[2,0] # Now we know how far l is from the plane. Next, compute the # distance object vertices to the plane, and use this information # to project each vertex onto the plane. for tri in self.triangles: for vert in tri.verts: v = vert.loc pv = v.minus(p1) b = array([[pv.dx],[pv.dy], [pv.dz]]) solution = solve(a,b) j = solution[2,0] # We know how far the vertex is from the plane. # Now project it onto the plane. lv = v.minus(l) ratio = (float) (d/(d-j)) vprime = l.plus(ratio*lv) # this is the projected point on the # plane self.shadows.append(vprime)
def add_target(self,ID,loc=None,IP=None): try: gx.TargetDic[ID] print 'Target with same ID already exists' return except: if loc is not None: t=gx.Target(ID,gm.point(loc),IP) else: t=gx.Target(ID,None,IP=IP) gx.TargetDic[ID]=t return t
def make_newtonian(focal_length, aperture_radius): """A very simple Newtonian design where we don't even stick in the flat secondary, because we're just trying to analyze coma (which isn't affected by the flat secondary). """ source = standard_source(focal_length, aperture_radius) aperture0 = CircularAperture(point(0, 0, focal_length), vector(0, 0, -aperture_radius)) # Reflector is z = r^2 / (4 focal length), so at edge, # we have: z_reflector_edge = aperture_radius**2 / (4 * focal_length) aperture1 = CircularAperture(point(0, 0, z_reflector_edge), vector(0, 0, -aperture_radius)) reflector = make_paraboloid(focal_length) # In practice there would be another mirror. So we actually pretend the sensor # is facing up. sensor = PlanarSensor.of_q_x_y(point(0, 0, focal_length), -ii, -jj, flip_z=True) elements = Compound([aperture0, aperture1, reflector]) return Instrument(source, elements, sensor)
def rebox(self, xscale=1.0, yscale=1.0, zscale=1.0): max_dims = point(sys.float_info.min, sys.float_info.min, sys.float_info.min) min_dims = point(sys.float_info.max, sys.float_info.max, sys.float_info.max) for v in self.all_vertices(): max_dims = max_dims.max(v.position) min_dims = min_dims.min(v.position) center = point((min_dims.x + max_dims.x) / 2.0, (min_dims.y + max_dims.y) / 2.0, (min_dims.z + max_dims.z) / 2.0) delta = max_dims - min_dims biggest = max(delta.dx, max(delta.dy, delta.dz)) scale = sqrt(2.0) / biggest for v in self.all_vertices(): offset = (v.position - center) * scale offset.dx *= xscale offset.dy *= yscale offset.dz *= zscale v.position = ORIGIN + offset
def read(cls,filename): obj_file = open(filename,'r') # Record the offset for vertex ID conversion. vertexi = len(vertex.all_instances()) # Count the number of vertex normals read. normali = 0 for line in obj_file: parts = line[:-1].split() if len(parts) > 0: # Read a vertex description line. if parts[0] == 'v': x = float(parts[1]) y = float(parts[2]) z = float(parts[3]) P = point(x,y,z) vertex.add(P) # Read a vertex normal description line. elif parts[0] == 'vn': dx = float(parts[1]) dy = float(parts[2]) dz = float(parts[3]) vn = vector(dx,dy,dz).unit() # vertex.with_id(normali).set_normal(vn) normali += 1 # Read a face/fan description line. elif parts[0] == 'f': #### ADDS AN OFFSET vertexi FROM THE .OBJ INDEX!!! (.OBJ starts at 1) #### vi_fan = [int(p.split('/')[0]) + vertexi - 1 for p in parts[1:]] vi1 = vi_fan[0] # add the faces of the fan for i in range(1,len(vi_fan)-1): vi2 = vi_fan[i] vi3 = vi_fan[i+1] V1 = vertex.with_id(vi1) V2 = vertex.with_id(vi2) V3 = vertex.with_id(vi3) face.add(V1,V2,V3) # rescale and center the points object.rebox()
def lse(cA): '''Returns a geometry.point() with estimated position. Raises ValueError if too few observations. ''' if len(cA) <= 1: raise ValueError('%s observations is too few' % len(cA)) l = len(cA) r = [w.r for w in cA] c = [w.c for w in cA] S = sum(r) W = [(S - w) / ((l - 1) * S) for w in r] p0 = gx.point(0, 0, 0) for i in range(l): p0 = p0 + W[i] * c[i] x0 = num.array([p0.x, p0.y]) res = minimize(sum_error, x0, args=(c, r), method='BFGS') return gx.point(res.x)
def read_pgm(self, filename): with open(filename, "r") as pgm_file: rows = 0 cols = 0 currRow = 1 currCol = 1 count = 1 for line in pgm_file: parts = line.split() # Set number of rows and columns if count <= 4: if len(parts) > 1 and rows == 0: if parts[0].isdigit() and parts[1].isdigit(): rows = int(parts[0]) cols = int(parts[1]) count += 1 # Read height data else: for p in parts: if not p.isdigit(): break # Make vertex height = int(p) P = point(currCol, height, currRow) vId = (currRow, currCol) self.make_vertex(vId, P) # Make 2 facets according to 2x2 square # with current vertex (vId) at position 11: # [ 00 01 ] # [ 10 11 ] if currRow > 1 and currCol > 1: vId00 = (currRow - 1, currCol - 1) vId01 = (currRow - 1, currCol) vId10 = (currRow, currCol - 1) self.make_face(vId, vId01, vId00) self.make_face(vId, vId00, vId10) if currCol == cols: currCol = 1 currRow += 1 else: currCol += 1 self.rebox(1.0, 0.5, 1.0) self.fix_edges()
def __init__(self,minX=-50,maxX=50,minY=-50,maxY=50,xPixels=600,timeStep=0.1): self.minX=minX self.maxX=maxX self.minY=minY self.maxY=maxY self.timeStep=timeStep self.aspect=(maxX-minX)/(maxY-minY) self.xPixels=xPixels self.yPixels=math.floor(xPixels/self.aspect) pygame.init() self.screen = pygame.display.set_mode((self.xPixels, self.yPixels)) self.bicycles=[] self.path=[] self.mouse_position=point(0,0)
def read(self,filename): obj_file = open(filename,'r') normali = 0 for line in obj_file: # Parse a line. parts = line[:-1].split() if len(parts) > 0: # Read a vertex description line. if parts[0] == 'v': x = float(parts[1]) y = float(parts[2]) z = float(parts[3]) P = point(x,y,z) vertex(P,self) # Read a vertex normal description line. elif parts[0] == 'vn': dx = float(parts[1]) dy = float(parts[2]) dz = float(parts[3]) vn = vector(dx,dy,dz).unit() self.vertex[normali].set_normal(vn) normali += 1 # Read a face/fan description line. elif parts[0] == 'f': vi_fan = [int(p.split('/')[0]) - 1 for p in parts[1:]] vi1 = vi_fan[0] # add the faces of the fan for i in range(1,len(vi_fan)-1): vi2 = vi_fan[i] vi3 = vi_fan[i+1] V1 = self.vertex[vi1] V2 = self.vertex[vi2] V3 = self.vertex[vi3] face(V1,V2,V3,self) # Wrap up the vertex fans. Re-chooses each vertex's out edge. self.finish() # Rescale and center the points. self.rebox()
def update_walls(self): for key, value in self.robot.world.particle_filter.sensor_model.landmarks.items( ): if key.startswith('Wall-'): if key in self.objects: wall = self.objects[key] if not wall.is_fixed and not wall.is_foreign: wall.update(self, x=value[0][0][0], y=value[0][1][0], theta=value[1]) else: print('Creating new wall in worldmap:', key) wall_spec = wall_marker_dict[key] wall = WallObj(id=key, x=value[0][0][0], y=value[0][1][0], theta=value[1], length=wall_spec.length, height=wall_spec.height, door_width=wall_spec.door_width, door_height=wall_spec.door_height, marker_specs=wall_spec.marker_specs, doorways=wall_spec.doorways, door_ids=wall_spec.door_ids, is_foreign=False, spec_id=key) self.objects[key] = wall wall.pose_confidence = +1 # Make the doorways wall.make_doorways(self.robot.world.world_map) # Relocate the aruco markers to their predefined positions spec = wall_marker_dict.get(wall.id, None) if spec is None: return for key, value in spec.marker_specs.items(): if key in self.robot.world.world_map.objects: aruco_marker = self.robot.world.world_map.objects[key] dir = value[0] # +1 for front side or -1 for back side s = 0 if dir == +1 else pi aruco_marker.theta = wrap_angle(wall.theta + s) wall_xyz = geometry.point( -dir * (wall.length / 2 - value[1][0]), 0, value[1][1]) rel_xyz = geometry.aboutZ(aruco_marker.theta + pi / 2).dot(wall_xyz) aruco_marker.x = wall.x + rel_xyz[0][0] aruco_marker.y = wall.y + rel_xyz[1][0] aruco_marker.z = rel_xyz[2][0] aruco_marker.is_fixed = wall.is_fixed
def lse(cA,mode='2D',cons=True): l=len(cA) r=[w.r for w in cA] c=[w.c for w in cA] S=sum(r) W=[(S-w)/((l-1)*S) for w in r] p0=gx.point(0,0,0) #Initialized point for i in range(l): p0=p0+W[i]*c[i] if mode=='2D' or mode=='Earth1': x0=num.array([p0.x,p0.y]) elif mode=='3D': x0=num.array([p0.x,p0.y,p0.z]) else: raise cornerCases, 'Mode not supported:'+mode if mode=='Earth1': fg1=1 else: fg1=0 if cons: print 'GC-LSE geolocating...' if not is_disjoint(cA,fg=fg1): cL=[] for q in range(l): def ff(x,q=q): return r[q]-Norm(x,c[q].std(),mode=mode) cL.append(ff) res = fmin_cobyla(sum_error, x0,cL,args=(c,r,mode),consargs=(),rhoend = 1e-5) ans=res else: raise cornerCases, 'Disjoint' else: print 'LSE Geolocating...' res = minimize(sum_error, x0, args=(c,r,mode), method='BFGS') ans=res.x return gx.point(ans)
def genTorus(smooth): innerR = 1 outerR = 3 smoothness = smooth numPoints = 2 * pi / smoothness points = [] facets = [] #generate the points of the sphere for i in range(0, smoothness): #latitude (goes from (pi/2) to (-pi/2)) theta = (i * numPoints - pi) for j in range(0, smoothness): #longitude (goes from -pi to pi) phi = j * numPoints # plug into parametrization x = (outerR + innerR * cos(theta)) * cos(phi) y = (outerR + innerR * cos(theta)) * sin(phi) z = innerR * sin(theta) points.append(point(x, y, z)) #generate the facets of the torus #rows for row in range(0, smoothness): for p in range(0, smoothness): k = (p + 1) % (smoothness) p1 = smoothness * row + p p2 = smoothness * row + k p3 = smoothness * (row - 1) + k p4 = smoothness * (row - 1) + p #top facets # / \ # --- #account for first row if row is not 0: facets.append([p4, p3, p2]) #bottom facets # --- # \ / facets.append([p1, p2, p4]) return points, facets
def update_carried_object(self, wmobject): #print('Updating carried object ',wmobject) # set x,y based on robot's pose # need to cache initial orientation relative to robot: # grasped_orient = wmobject.theta - robot.pose.rotation.angle_z world_frame = self.robot.kine.joints['world'] lift_attach_frame = self.robot.kine.joints['lift_attach'] tmat = self.robot.kine.base_to_link(world_frame).dot( self.robot.kine.joint_to_base(lift_attach_frame)) # *** HACK *** : depth calculation only works for cubes; need to handle custom obj, chips half_depth = wmobject.size[0] / 2 new_pose = tmat.dot(geometry.point(half_depth, 0)) theta = self.robot.world.particle_filter.pose[2] wmobject.x = new_pose[0, 0] wmobject.y = new_pose[1, 0] wmobject.z = new_pose[2, 0] wmobject.theta = theta
def CCA(cA,mode='2D',detail=False): if mode=='2D': from shapely_2D import polygonize elif mode=='Earth1': from shapely_earth1 import polygonize else: print """The combination of centroid method and your selected mode does not exist""" raise cornerCases, 'InputError' P=polygonize([xx.c for xx in cA],[xx.r for xx in cA]) area,n=maxPol(P) ans1=area.centroid ans=gx.point(ans1.x,ans1.y) if detail: return (ans,n,P,area) else: return (ans,n)
def CCA(cA,mode='2D',detail=False): if mode=='2D': from shapely_2D import polygonize elif mode=='Earth1': from shapely_earth1 import polygonize else: print("""The combination of centroid method and your selected mode does not exist""") raise cornerCases('InputError') P=polygonize([xx.c for xx in cA],[xx.r for xx in cA]) area,n=maxPol(P) ans1=area.centroid ans=gx.point(ans1.x,ans1.y) if detail: return (ans,n,P,area) else: return (ans,n)
def getRasterMinMax(self, subTrack, raster): pointList = self.pointLists[subTrack] minRaster = 9999999 maxRaster = -9999999 akkuVal = 0 for iP in range(len(pointList)): p1 = pointList[iP].obj.GetGeometryRef().GetPoint(0) p1 = point(p1[0], p1[1]) rasterVal = raster.getCellValueAtGeolocation(p1.x, p1.y) akkuVal += rasterVal if iP == 0: startVal = rasterVal if rasterVal < minRaster: minRaster = rasterVal if rasterVal > maxRaster: maxRaster = rasterVal endVal = rasterVal return (startVal, endVal, minRaster, maxRaster, akkuVal / len(pointList))
def ray_intersect(self, ray1): """ Computes intersection of this triangle facet and ray RAY1. MINUS_Z is a vector along the negative-z axis into the scene. returns a point on the triangle if they intersect, else returns none. """ normalVec = self.normal.scale(-1) if (ray1.towards.dot(normalVec)<=1.0e-8): return None # the ray is parallel to the triangle, # thus, it either misses or hits the triangle edge-on. # The intersection of ray1 and this triangle is t times the # vector (ray1.source + unit). This is some point on the triangle, # t is computed as follows: t = -(normalVec.dot(ray1.source - self.verts[0].loc)) / (normalVec.dot(ray1.towards)) P = ray1.source.plus(ray1.towards.scale(t)) # Point on plane that ray1 hits Pmat = array([[P.x], [P.y], [P.z]]) # t, in numpy format M = array([[self.verts[0].loc.x, self.verts[1].loc.x, self.verts[2].loc.x], [self.verts[0].loc.y, self.verts[1].loc.y, self.verts[2].loc.y], [self.verts[0].loc.z, self.verts[1].loc.z, self.verts[2].loc.z]]) try: solution = solve(M, Pmat) except ValueError: if ValueError == LinAlgError: print("Linear Algebra Error \n") return None barycentric = point(solution[0,0], solution[1,0], solution[2,0]) if barycentric.x < 0.0 or barycentric.y < 0.0 or barycentric.z < 0.0: return None if barycentric.x > 1.0 or barycentric.y > 1.0 or barycentric.z > 1.0: return None return P
def genCylinder(smooth): smoothness = smooth radius = 2 height = 5 numPoints = 2 * pi / smoothness points = [] facets = [] #generate the points of the sphere for i in range(0, smoothness): #latitude (goes from (pi/2) to (-pi/2)) for j in range(0, smoothness): #longitude (goes from -pi to pi) phi = j * numPoints # cylindrical coordinates ==> R^3 x = radius * cos(phi) y = radius * sin(phi) z = i * (height / smoothness) points.append(point(x, y, z)) #generate the facets of the sphere #rows for row in range(0, smoothness): for p in range(0, smoothness): k = (p + 1) % (smoothness) p1 = smoothness * row + p p2 = smoothness * row + k p3 = smoothness * (row - 1) + k p4 = smoothness * (row - 1) + p #top facets # / \ # --- #bottom facets # --- # \ / if row is not 0: facets.append([p4, p3, p2]) facets.append([p1, p2, p4]) return points, facets
def mouse(button, state, x, y): wx, wy = world(x,y) wp = point(wx,wy,0.0) if not GLOBALS.paused: return if state == MOUSE_DOWN: if GLOBALS.selected != None and GLOBALS.why_selected == MASS_MOVEMENT: # If it's the second click in a mouse movement, let go. GLOBALS.selected.set_position(wp) GLOBALS.selected = None elif GLOBALS.selected != None and GLOBALS.why_selected == SPRING_SELECTED: GLOBALS.selected = None elif GLOBALS.selected == None: # If it's a fresh click then... how_close_mass, which_mass = Springies.closest_mass_to(wp) how_close_spring, which_spring = Springies.closest_spring_to(wp) if which_mass != None and how_close_mass < MASS_THRESHOLD: # ...start moving a mass. GLOBALS.selected = which_mass GLOBALS.why_selected = MASS_MOVEMENT elif which_spring != None and how_close_spring < SPRING_THRESHOLD: # ...select a spring for editing. GLOBALS.selected = which_spring GLOBALS.why_selected = SPRING_SELECTED else: # ...otherwise create a new mass and start moving it. Mass(1.0,wp) elif state == MOUSE_UP: if GLOBALS.selected != None and GLOBALS.why_selected == SPRING_BUILDING: # If we're building a spring, see if we're near an existing mass. mass1 = GLOBALS.selected how_close_mass, which_mass = Springies.closest_mass_to(wp) if how_close_mass != None and which_mass != mass1 and how_close_mass < MASS_THRESHOLD: mass2 = which_mass Spring(mass1,mass2,1000.0) GLOBALS.selected = None else: GLOBALS.selected = None glutPostRedisplay()
def make_arucos(self, world_map): "Called by add_fixed_landmark to make fixed aruco markers." for key, value in self.marker_specs.items(): # Project marker onto the wall; move marker if it already exists marker = world_map.objects.get(key, None) if marker is None: marker_number = int(key[1 + key.rfind('-'):]) marker = ArucoMarkerObj(world_map.robot.world.aruco, marker_number=marker_number) world_map.objects[marker.id] = marker wall_xyz = geometry.point(self.length / 2 - value[1][0], 0, value[1][1]) s = 0 if value[0] == +1 else pi rel_xyz = geometry.aboutZ(self.theta + s).dot(wall_xyz) marker.x = self.x + rel_xyz[1][0] marker.y = self.y + rel_xyz[0][0] marker.z = rel_xyz[2][0] marker.theta = wrap_angle(self.theta + s) marker.is_fixed = self.is_fixed
def drag(x, y): wx, wy = world(x,y) wp = point(wx,wy,0.0) GLOBALS.highlighted = None if GLOBALS.selected != None and GLOBALS.why_selected == SPRING_BUILDING: how_close_mass, which_mass = Springies.closest_mass_to(wp) if how_close_mass < MASS_THRESHOLD: GLOBALS.highlighted = which_mass else: GLOBALS.last_mouse = wp elif GLOBALS.selected != None and GLOBALS.why_selected == MASS_MOVEMENT: if (wp - GLOBALS.selected.get_position()).norm() > 0.1: GLOBALS.why_selected = SPRING_BUILDING GLOBALS.last_mouse = wp glutPostRedisplay()
def __init__(s): """ Initialize the plane, at 10000 feet going 150kts. """ # Parameters related to airplane's position # All parameters are in feet. # Some starting parameters s.altitude = ft2WU(12000) # in ft s.airspeed = kts2WUps(200) # in kts s.AngleOfAttack = 0.0 # in degrees s.Pilot = point(0.0, s.altitude, 0.0) s.Nose = s.Pilot + vector(0.0, 0.0, 1.0) s.Up = s.Pilot + vector(0.0, 1.0, 0.0) s.velocity = vector(0.0, 0.0, 1.0).scale(s.airspeed) ### OK. Init the rigid body. # The rigid body is the plane itself - it handles forces # incoming from this object and comptues the plane's new position. s.rigid = rigidBody(s.Pilot, 100, s.velocity) # Add forces to it: s.rigid.addForce(s.thrust) s.rigid.addForce(s.drag) s.rigid.addForce(s.gravity(s.rigid)) s.rigid.addPointForce(s.leftWing, 'left') s.rigid.addPointForce(s.rightWing, 'right') s.rigid.addPointForce(s.elevator, 'tail') s.rigid.addPointForce(s.rudder, 'tail') s.rigid.addPointForce(s.stabilizer, 'tail') s.warning = False # whether or not to flash the warning lamp # Control parameters s.x = 0.0 # The mouse/joystick x coord (-0.5 to 0.5) s.y = 0.0 # Mouse/joystick y coord (-0.5 to 0.5) s.r = 0.0 # Keyboard rudder control (-0.5 or 0.5)
def addPoints(p0, p1): """ Adds two points, p1 and p0, component wise. """ return point(p0.x + p1.x, p0.y + p1.y, p0.z + p1.z)
from geometry import point, vector, EPSILON, ORIGIN from random import random from math import sin, cos, acos, pi, sqrt, factorial from OpenGL.GLUT import * from OpenGL.GL import * width = 500 height = 500 ctl_pts = None m_pts = None k_degree = 2 radius = 5.0 pointSize = 14 iproj = None org = point(0.0,0.0,0.0) smoothness = 10 def init(): global ctl_pts, m_pts #set the control points for a triangle c0 = point((radius-1),0,0) c1 = point((1-radius),0,0) c2 = point(0,(radius-1),0) ctl_pts = [c0, c1, c2] #subdivide into three curves m = [] = array of midpoints m0 = c0.plus(c1.minus(c0).scale(.5)) m1 = c1.plus(c2.minus(c1).scale(.5))
def scalePoint(p, scalar): """ Scales all coordinates of the point by scalar and returns. the resulting point. """ return point(scalar*p.x, scalar*p.y, scalar*p.z)
def refine(self): selfie = object() vclones = {} vnew = {} #create four faces: splitting phase for f in self.face: nvpts = [] for edge in f.edges(): if edge.source not in vclones: vclones[edge.source] = vertex(edge.source.position, selfie) #make the new vertex nvec = edge.vector().scale(1/2) nvpos = edge.source.position.plus(nvec) nv = vertex(nvpos, selfie) if edge.twin is not None and edge.twin in vnew: nvp = vnew[edge.twin] nvpts.append(nvp) selfie.vertex.pop() #add the vertex introduced at this edge #the edge is the key, the new vertex is the value elif edge.twin not in vnew: vnew[edge] = nv nvpts.append(vnew[edge]) else: print("error") #create the faces for i in range(0,3): face(vclones[f.edge(i).source], nvpts[i], nvpts[(i+2)%3], selfie) face(nvpts[0], nvpts[1], nvpts[2], selfie) selfie.finish() #Debugging # print ("number of faces in self.face: " + str(len(self.face))) # print ("number of faces in selfie.face: " + str(len(selfie.face))) # print ("number of edges in self.edge: " + str(len(self.edge))) # print ("number of edges in selfie.edge: " + str(len(selfie.edge))) # print ("number of vertices in self.vertex: " + str(len(self.vertex))) # print ("number of vertices in selfie.vertex: " + str(len(selfie.vertex))) # averaging phase # recompute the position of all of the vertices that were part of the original shape for v in vclones: sv = vclones[v] ps = [e.vertex(1).position for e in sv.around()] count = len(ps) ss = [1.0/count] * count centroid = v.position.combos(ss,ps) #we're on a cone if sv.edge.twin is not None: bn = 5.0/8.0 - (3.0/8.0 + 2.0*cos((2.0*pi)/count)/8.0) ** 2 yn = bn / (1.0 - bn) an = (yn/(1.0+yn)) dn = (1.0/(1.0+yn)) # sve = sv.position.components() fvec = [x*an + y*dn for x,y in zip(sv.position.components(), centroid)] sv.position = point(fvec[0], fvec[1], fvec[2]) #otherwise, we're on a fan else: vn1 = cur.source.position.minus(ORIGIN).scale(1/8) v0 = sv.edge.vertex(1).position.minus(ORIGIN).scale(1/8) vp = sv.edge.vector().scale(3/4) nvec = vp.plus(v0).plus(vn1) sv.position = point(nvec[0], nvec[1], nvec[2]) #recompute the position of the vertices that are new for v in vnew.values(): e0 = v.edge #case 1: interior edge on boundary if e0.twin is None: fvec = [(1/2)*(x + y) for x,y in zip(v.position.components(), e0.vertex(1).position.components())] v.position = point(fvec[0], fvec[1], fvec[2]) #case 2: split interior edge else: e1 = e0.twin v1 = e1.source f0 = e0.next.next.source f1 = e1.next.next.source fs = [(1/8) * (x+y) for x, y in zip(f1.position.components(), f0.position.components())] vs = [(3/8) * (x+y) for x, y in zip(v.position.components(), v1.position.components())] fvec = [x + y for x, y in zip(fs, vs)] v.position = point(fvec[0], fvec[1], fvec[2]) return selfie
def getP(s): """ Returns the position, scaled to world units """ return point(s.P.x*s, s.P.y*s, s.P.z*s)