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)
