def CoordinateRotation_3D(x, y, z, theta, alpha):
v1 = vp.vector(x, y, z)
# Rotate about y'-axis for x' to x and z' to z
theta_radians = math.radians(theta) # theta is radians of measured degrees
v_abouty = vp.rotate(v1, angle=theta_radians, axis=vp.vector(0, 1, 0))
# Rotate about x-axis for y' to y and z' to z
alpha_radians = math.radians(alpha)
v_aboutx = vp.rotate(v_abouty,
angle=alpha_radians,
axis=vp.vector(1, 0, 0))
return v_aboutx
def motion_no_drag(data):
"""
Create animation for projectile motion with no dragging force
"""
# ball is cyan and has a trail.
ball_nd = sphere(pos=vector(-80, data['init_height'], 0),
radius=1,
color=color.cyan,
make_trail=True)
# Follow the movement of the ball
scene.camera.follow(ball_nd)
# Set initial velocity & position.
# set initial velosity to go in the x direcion and rotate via angle. rotate requres radians
ball_nd.velocity = rotate(vector(data['init_velocity'], 0, 0),
angle=radians(data['theta']))
# set the balls mass
ball_nd.mass = data['ball_mass']
# set the balls momentum
ball_nd.p = ball_nd.velocity * ball_nd.mass
# set the force of gravaity vector
netForce = vector(0, data['gravity'], 0) * ball_nd.mass
# time is 0
t = 0
# Animate
while ball_nd._pos.y > ball_nd.radius:
rate(300)
# plot via velosty tims deltat + position
ball_nd.pos = ball_nd.pos + (ball_nd.p / ball_nd.mass) * data['deltat']
# set new monmentum
ball_nd.p = ball_nd.p + netForce * data['deltat']
# set new time
t = t + data['deltat']
def set_thrust(t, bodies):
if t > 0 and t < 1:
bodies[0].force = 100 * vp.rotate(vp.vector(1, 0, 0), bodies[0].theta, vp.vector(0, 0, 1))
else:
bodies[0].force = vp.vector(0, 0, 0)
bodies[1].force = vp.vector(0, 0, 0)
def cylinder(radius=1, height=2, center=vpy.vec(0, 0, 0), segments=90):
"""
create a cylinder aligned with the vertical (z-)axis.
radius or the base and height are the given parameters
"""
if segments < 3:
raise ValueError(
f"Number of segments cannot be below 3 but was {segments}.")
rot_vec = vpy.vec(radius, 0, 0)
axis = vpy.vec(0, 0, 1)
corners_1 = list()
corners_2 = list()
for _ in range(segments):
corners_1.append(rot_vec - vpy.vec(0, 0, height / 2))
corners_2.append(rot_vec + vpy.vec(0, 0, height / 2))
rot_vec = vpy.rotate(rot_vec, axis=axis, angle=2 * vpy.pi / segments)
# corners_2.append(point_2.rotate(axis=axis, angle=2*vpy.pi/segments, origin=center))
corners = corners_1 + corners_2
faces = [list(range(segments)), list(range(segments, 2 * segments))]
for i in range(segments):
p1 = i
p2 = (i + 1) % segments
p3 = (i + 1) % segments + segments
p4 = i + segments
faces.append([p1, p2, p3, p4])
return corners, faces
def draw(self):
axis, theta = _euler.euler2axangle(self.pqr.x, self.pqr.y, self.pqr.z)
axis = _vp.vector(axis[0], axis[1], axis[2])
up = _vp.rotate(_vp.vector(0,1,0), theta, axis)
self.body.pos = self.xyz
self.top.pos = self.xyz + up*_size
self.prop1.pos = self.xyz + _vp.rotate(_vp.vector(1.3*_size,0,0), theta, axis)
self.prop2.pos = self.xyz + _vp.rotate(_vp.vector(0,0,1.3*_size), theta, axis)
self.prop3.pos = self.xyz + _vp.rotate(_vp.vector(-1.3*_size,0,0), theta, axis)
self.prop4.pos = self.xyz + _vp.rotate(_vp.vector(0,0,-1.3*_size), theta, axis)
self.prop1.axis = up
self.prop2.axis = up
self.prop3.axis = up
self.prop4.axis = up
if _follow_drone:
canvas.center = self.xyz
canvas.caption = 'time = %0.1f, pos = (%0.1f, %0.1f, %0.1f), energy = %0.1f' % (time, self.xyz.x, self.xyz.y, self.xyz.z, self.energy)
def update(self, dt):
# forces
axis, theta = _euler.euler2axangle(self.pqr.x, self.pqr.y, self.pqr.z)
axis = _vp.vector(axis[0], axis[1], axis[2])
up = _vp.rotate(_vp.vector(0,1,0), theta, axis)
a = _vp.vector(0, -_gravity, 0)
a = a + (self.thrust1+self.thrust2+self.thrust3+self.thrust4)/self.mass * up + self.wind/self.mass
a = a - (_lin_drag_coef * _vp.mag(self.xyz_dot)**2)/self.mass * self.xyz_dot
self.xyz_dot = self.xyz_dot + a * dt
# torques (ignoring propeller torques)
cg = self.cgpos * up
tpos1 = _vp.rotate(_vp.vector(1.3*_size,0,0), theta, axis)
tpos2 = _vp.rotate(_vp.vector(0,0,1.3*_size), theta, axis)
tpos3 = _vp.rotate(_vp.vector(-1.3*_size,0,0), theta, axis)
tpos4 = _vp.rotate(_vp.vector(0,0,-1.3*_size), theta, axis)
torque = _vp.cross(cg, _vp.vector(0, -_gravity, 0))
torque = torque + _vp.cross(tpos1, self.thrust1 * up)
torque = torque + _vp.cross(tpos2, self.thrust2 * up)
torque = torque + _vp.cross(tpos3, self.thrust3 * up)
torque = torque + _vp.cross(tpos4, self.thrust4 * up)
torque = torque - _rot_drag_coef * self.pqr_dot
aa = torque/self.inertia
if _vp.mag(aa) > 0:
aai, aaj, aak = _euler.axangle2euler((aa.x, aa.y, aa.z), _vp.mag(aa))
aa = _vp.vector(aai, aaj, aak)
self.pqr_dot = self.pqr_dot + aa * dt
else:
self.pqr_dot = _vp.vector(0,0,0)
# ground interaction
if self.xyz.y <= 0:
self.xyz.y = 0
if self.xyz_dot.y <= 0:
self.xyz_dot.x = self.xyz_dot.x * _ground_friction
self.xyz_dot.y = 0
self.xyz_dot.z = self.xyz_dot.z * _ground_friction
self.pqr_dot = self.pqr_dot * _ground_friction
# energy update
self.energy += _power_coef * (self.thrust1**1.5 + self.thrust2**1.5 + self.thrust3**1.5 + self.thrust4**1.5) * dt
# time update
self.xyz += self.xyz_dot * dt
self.pqr += self.pqr_dot * dt
# callback
if self.updated is not None:
self.updated(self)
self.draw()
def move(self):
"""Moves particle towards (0, 0, 0). Also moves randomly perpendicular
to this trajectory.
"""
self.steps += 1
centre_vector = vp.norm(vp.vector(0, 0, 0) - self.pos)
perp_vector = vp.rotate(centre_vector, angle=math.pi / 2)
self.pos += centre_speed * centre_vector
self.pos += 0.2 * random.uniform(-1, 1) * perp_vector
def vertices(body):
body_vertices = [vp.vector(body.width/2, body.height/2, 0),
vp.vector(-body.width/2, body.height/2, 0),
vp.vector(-body.width/2, -body.height/2, 0),
vp.vector(body.width/2, -body.height/2, 0)]
for idx, pt in enumerate(body_vertices):
body_vertices[idx] = vp.rotate(pt, angle=body.theta, axis=vp.vector(0, 0, 1)) + body.pos
return body_vertices
def updateValues(self):
simTime = moose.element('/clock').currentTime
#self.timeStr.set_text( "Time= {:.3f}".format( time ) )
for i in self.drawables_:
i.updateValues(simTime)
if self.doRotation and abs(self.rotation) < 2.0 * 3.14 / 3.0:
self.scene.forward = vp.rotate(self.scene.forward,
angle=self.rotation,
axis=self.scene.up)
self.updateAxis()
if self.viewIdx == 0:
self.timeLabel.text = "Time = {:7.3f} sec".format(simTime)
vp.sleep(self.sleep)
def integrate(dt, roller, alpha):
slope_dir = vp.norm(vp.rotate(vp.vector(1, 0, 0), angle=-alpha))
# Rolling with sliding (movie - 13:21:00)
# a = g(sin(a) - u*cos(a)) - page 104
# roller.acc = GRAVITY_ACC * (math.sin(alpha) - roller.friction_coeff * math.cos(alpha)) * slope_dir
# Rolling without sliding - (movie - 10:24:00)
roller.acc = (roller.mass * GRAVITY_ACC * math.sin(alpha)) / (
roller.mass + roller.moment / roller.radius**2) * slope_dir
roller.vel += roller.acc * dt
roller.pos += roller.vel * dt
roller.ang_vel += ((roller.force * roller.radius) / roller.moment) * dt
angle_diff = roller.ang_vel * dt
# Minus, because function (vp.rotate) rotate counter-clockwise
roller.rotate(angle=-angle_diff)
def rot_SD_u_mean(SD_init, user, rot_angle): user = array(user) angle = radians(float(rot_angle)) #rot_axe = vector(rot_axe[0],rot_axe[1],rot_axe[2]) SD_rotated = empty(shape(SD_init)) i = 0 for sd in SD_init: sd_v = vector(sd[0], sd[1], sd[2]) p0 = Point3D(0.0, 0.0, 0.0) p1 = Point3D(sd[0], sd[1], sd[2]) p2 = Point3D(user[0], user[1], user[2]) plane_actual = Plane(p0, p1, p2) rot_axe = plane_actual.normal_vector rot_axe = [rot_axe[0].n(5),rot_axe[1].n(5),rot_axe[2].n(5)] #print(rot_axe) rot_axe = vector(float(rot_axe[0]), float(rot_axe[1]), float(rot_axe[2])) #print(angle) sd_r = rotate(sd_v, angle=angle, axis=rot_axe) SD_rotated[i] = array([sd_r.x, sd_r.y, sd_r.z]) i += 1 return SD_rotated
def rot_SD(SD_init, rot_axe, rot_angle):
if rot_axe == "X":
rot_axe = [1.0,0.0,0.0]
elif rot_axe == "Y":
rot_axe = [0.0,1.0,0.0]
elif rot_axe == "Z":
rot_axe = [0.0,0.0,1.0]
rot_axe = vector(rot_axe[0],rot_axe[1],rot_axe[2])
angle = radians(float(rot_angle))
SD_rotated = empty(shape(SD_init))
i = 0
for sd in SD_init:
sd_v = vector(sd[0], sd[1], sd[2])
#print(sd, '-sd\n', sd_v.x, sd_v.y, sd_v.z)
#===== rotate
sd_r = rotate(sd_v, angle=angle, axis=rot_axe)
#print( sd_r.x, sd_r.y, sd_r.z)
SD_rotated[i] = array([sd_r.x, sd_r.y, sd_r.z])
#print(dot(SD_rotated[i], sd))
i += 1
#print('dot() of SDs: ', sum(dot(SD_init[0], SD_rotated[0])))
return SD_rotated
def create_bodies():
ramp = vp.box(pos=vp.vector(0, -0.25, 0),
length=5,
width=2.5,
height=0.5,
alpha=vp.radians(15))
# Minus, because function (vp.rotate) rotate counter-clockwise
ramp.rotate(angle=-ramp.alpha, axis=vp.vector(0, 0, 1))
roller = vp.cylinder(pos=vp.vector(
-1 * math.cos(ramp.alpha) * 0.5 * ramp.length,
math.sin(ramp.alpha) * 0.5 * ramp.length + 0.5, -1),
axis=vp.vector(0, 0, 2),
radius=0.5,
texture=texture(),
vel=vp.vector(0, 0, 0),
ang_vel=0,
mass=1,
friction_coeff=0.15)
slope_dir = vp.norm(vp.rotate(vp.vector(1, 0, 0), angle=-ramp.alpha))
arrow = vp.arrow(pos=vp.vector(0, 2, 1), axis=slope_dir)
return roller, ramp, arrow
def motion_drag(data):
"""
Create animation for projectile motion with dragging force
"""
ball_nd = sphere(pos=vector(-80, data['init_height'], 0),
radius=1,
color=color.purple,
make_trail=True)
# Follow the movement of the ball
scene.camera.follow(ball_nd)
# Set initial velocity & position.
# set initial velosity to go in the x direcion and rotate via angle. rotate requres radians
ball_nd.velocity = rotate(vector(data['init_velocity'], 0, 0),
angle=radians(data['theta']))
ball_nd.mass = data['ball_mass']
# set the balls momentum
ball_nd.p = ball_nd.velocity * ball_nd.mass
# foce of gravaty - force of air fiction (alpha*velosity^2) * a vector of 1 in the balls direction of movement.
netForce = ((vector(0, data['gravity'], 0) * ball_nd.mass) -
(data['alpha'] * mag(ball_nd.p / ball_nd.mass)**2 *
(ball_nd.p / mag(ball_nd.p))))
t = 0
# Animate
while ball_nd._pos.y > ball_nd.radius:
rate(300)
# plot via velosty tims deltat + position
ball_nd.pos = ball_nd.pos + (ball_nd.p / ball_nd.mass) * data['deltat']
# air friction changes with speed of the ball.
netForce = ((vector(0, data['gravity'], 0) * ball_nd.mass) -
(data['alpha'] * mag(ball_nd.p / ball_nd.mass)**2 *
(ball_nd.p / mag(ball_nd.p))))
# set new monmentum
ball_nd.p = ball_nd.p + netForce * data['deltat']
# set new time
t = t + data['deltat']
def _create_hab(self, entity: Entity, texture: str) -> \
vpython.compound:
def vertex(x: float, y: float, z: float) -> vpython.vertex:
return vpython.vertex(pos=vpython.vector(x, y, z))
# See the docstring of ThreeDeeObj._create_obj for why the dimensions
# that define the shape of the habitat will not actually directly
# translate to world-space.
body = vpython.cylinder(pos=vpython.vec(0, 0, 0),
axis=vpython.vec(-5, 0, 0),
radius=10)
head = vpython.cone(pos=vpython.vec(0, 0, 0),
axis=vpython.vec(2, 0, 0),
radius=10)
wing = vpython.triangle(v0=vertex(0, 0, 0),
v1=vertex(-5, 30, 0),
v2=vertex(-5, -30, 0))
wing2 = vpython.triangle(v0=vertex(0, 0, 0),
v1=vertex(-5, 0, 30),
v2=vertex(-5, 0, -30))
hab = vpython.compound([body, head, wing, wing2],
make_trail=True,
texture=texture)
hab.axis = calc.angle_to_vpy(entity.heading)
hab.radius = entity.r / 2
hab.shininess = 0.1
hab.length = entity.r * 2
boosters: List[vpython.cylinder] = []
body_radius = entity.r / 8
for quadrant in range(0, 4):
# Generate four SRB bodies.
normal = vpython.rotate(vpython.vector(0, 1, 1).hat,
angle=quadrant * vpython.radians(90),
axis=vpython.vector(1, 0, 0))
boosters.append(
vpython.cylinder(radius=self.BOOSTER_RADIUS,
pos=(self.BOOSTER_RADIUS + body_radius) *
normal))
boosters.append(
vpython.cone(
radius=self.BOOSTER_RADIUS,
length=0.2,
pos=((self.BOOSTER_RADIUS + body_radius) * normal +
vpython.vec(1, 0, 0))))
# Append an invisible point to shift the boosters down the fuselage.
# For an explanation of why that matters, read the
# ThreeDeeObj._create_obj docstring (and if that doesn't make sense,
# get in touch with Patrick M please hello hi I'm always free!)
boosters.append(vpython.sphere(opacity=0, pos=vpython.vec(1.2, 0, 0)))
booster_texture = texture.replace(f'{entity.name}.jpg', 'SRB.jpg')
hab.boosters = vpython.compound(boosters, texture=booster_texture)
hab.boosters.length = entity.r * 2
hab.boosters.axis = hab.axis
parachute: List[vpython.standardAttributes] = []
string_length = entity.r * 0.5
parachute_texture = texture.replace(f'{entity.name}.jpg',
'Parachute.jpg')
# Build the parachute fabric.
parachute.append(
vpython.extrusion(
path=vpython.paths.circle(radius=0.5, up=vpython.vec(-1, 0,
0)),
shape=vpython.shapes.arc(angle1=vpython.radians(5),
angle2=vpython.radians(95),
radius=1),
pos=vpython.vec(string_length + self.PARACHUTE_RADIUS / 2, 0,
0)))
parachute[0].height = self.PARACHUTE_RADIUS * 2
parachute[0].width = self.PARACHUTE_RADIUS * 2
parachute[0].length = self.PARACHUTE_RADIUS
for quadrant in range(0, 4):
# Generate parachute attachment lines.
string = vpython.cylinder(axis=vpython.vec(string_length,
self.PARACHUTE_RADIUS,
0),
radius=0.2)
string.rotate(angle=(quadrant * vpython.radians(90) -
vpython.radians(45)),
axis=vpython.vector(1, 0, 0))
parachute.append(string)
parachute.append(
vpython.sphere(opacity=0,
pos=vpython.vec(
-(string_length + self.PARACHUTE_RADIUS), 0,
0)))
hab.parachute = vpython.compound(parachute, texture=parachute_texture)
hab.parachute.visible = False
return hab
def solve_scene(kb_db_name, obj_file_path):
"""
Solve manipulation for the target object in the scene using analogy.
Args:
kb_db_name(str): The knowledge base database file name.
obj_file_path(str): File path to the .obj file.
"""
# create vpython scene that is used for graphical representation of a scene
# for the user
vpython_scene = vpython.canvas(
title='',
width=800,
height=600,
center=vpython.vector(0, 0, 0),
background=vpython.color.black)
# Draw XYZ axis in the scene
vpython_drawings.draw_xyz_arrows(300.0)
scene = file_parsers.read_obj_file(obj_file_path)
# collision detection for each mesh in the scene
min_distance = 3
for mesh_1 in scene.values():
for mesh_2 in scene.values():
if mesh_1.name != mesh_2.name:
aabb_collision = aabb_col.aabb_intersect(
mesh_1, mesh_2, min_distance=min_distance)
if aabb_collision:
for surface in mesh_1.surfaces:
intersect = aabb_col.aabb_intersect_vertex(
mesh_2, surface.collider, min_distance=min_distance)
if intersect:
surface.collided_objects[mesh_2.name] = True
surface.collision = True
mesh_1.collision = True
mesh_1.collided_objects[mesh_2.name] = True
for side, surfaces in mesh_1.aabb.closest_surfaces.items():
counter = 0
for surface in surfaces:
if surface.collision:
counter += 1
if counter == len(surfaces):
mesh_1.aabb.collided_sides[side] = 2
elif counter > 0 and counter < len(surfaces):
mesh_1.aabb.collided_sides[side] = 1
# vpython_drawings.draw_colliders(mesh_list=scene.values())
vpython_drawings.draw_aabb_colliders(mesh_list=scene.values())
# vpython_drawings.draw_mesh(mesh_list=scene.values())
vpython_drawings.draw_aabb(mesh_list=scene.values(), opacity=0.4)
# select target object
picked_vpython_obj = user_inputs.select_object(vpython_scene)
picked_obj = scene[picked_vpython_obj.name[:-5]]
# get aabbs from KB
db = sqlitedb.sqlitedb(name=kb_db_name)
db_aabbs = db.select_all_aabbs()
# rebuild aabb from db as an AABB object
aabbs = {}
for db_aabb in db_aabbs:
# get positions of manipulation points and vector forces from DB
pos = db.select_position_id(db_aabb[2])
push_point = db.select_position_id(
int(db.select_push_point_id(int(db_aabb[3]))[1]))
push_vec = db.select_push_vec_id(int(db_aabb[4]))
pull_point = db.select_position_id(
int(db.select_pull_point_id(int(db_aabb[5]))[1]))
pull_vec = db.select_pull_vec_id(int(db_aabb[6]))
spatula_point = db.select_position_id(
int(db.select_spatula_point_id(int(db_aabb[7]))[1]))
spatula_vec = db.select_spatula_vec_id(int(db_aabb[8]))
# Create AABB object and assign collided sides
aabb = AABB([pos[1], pos[2], pos[3]],
[db_aabb[9], db_aabb[10], db_aabb[11]])
aabb.collided_sides['top'] = db_aabb[12]
aabb.collided_sides['bottom'] = db_aabb[13]
aabb.collided_sides['front'] = db_aabb[14]
aabb.collided_sides['back'] = db_aabb[15]
aabb.collided_sides['right'] = db_aabb[16]
aabb.collided_sides['left'] = db_aabb[17]
# assign manipulation points and force vectors for the AABB object
if push_point[1] is not None:
aabb.manipulation_points['push'] = [
float(push_point[1]),
float(push_point[2]),
float(push_point[3])
]
aabb.manipulation_vectors['push'] = [
int(push_vec[1]),
int(push_vec[2]),
int(push_vec[3])
]
else:
aabb.manipulation_points['push'] = None
aabb.manipulation_vectors['push'] = None
if pull_point[1] is not None:
aabb.manipulation_points['pull'] = [
float(pull_point[1]),
float(pull_point[2]),
float(pull_point[3])
]
aabb.manipulation_vectors['pull'] = [
int(pull_vec[1]),
int(pull_vec[2]),
int(pull_vec[3])
]
else:
aabb.manipulation_points['pull'] = None
aabb.manipulation_vectors['pull'] = None
if spatula_point[1] is not None:
aabb.manipulation_points['spatula'] = [
float(spatula_point[1]),
float(spatula_point[2]),
float(spatula_point[3])
]
aabb.manipulation_vectors['spatula'] = [
int(spatula_vec[1]),
int(spatula_vec[2]),
int(spatula_vec[3])
]
else:
aabb.manipulation_points['spatula'] = None
aabb.manipulation_vectors['spatula'] = None
aabbs[db_aabb[0]] = aabb
# get all mappings and their scores. Sort them based on highest score
analogy_mapping = Mapping()
mappings_scores = []
for aabb_id, aabb in aabbs.items():
new_aabb_scores = analogy_mapping.get_mappings_score(
target_aabb=picked_obj.aabb, source_aabb=aabb)
# save only top 3 rotation sequences based on highest score
top_3_scores = heapq.nlargest(
3, new_aabb_scores, key=operator.itemgetter(0))
# The result is saved as tuple in a form
# (aabb_id, top score from all 3, list of best 3 rotation sequences with their scores)
mappings_scores.append((aabb_id, top_3_scores[0][0], top_3_scores))
# sort the mapping scores based on the top scores
mappings_scores = sorted(
mappings_scores, key=operator.itemgetter(1), reverse=True)
# print out info about best mapping
print(':' * 120)
for aabb_mapping in mappings_scores:
print('=' * 120)
print('aabb id:', aabb_mapping[0], 'scene:',
db.select_aabb_id(aabb_mapping[0])[1])
print('max score for the AABB:', aabb_mapping[1])
for score in aabb_mapping[2]:
print('-' * 120)
print('score:', score[0], '\nsequence:', score[1], '\nmapping:',
analogy_mapping.all_permutations[score[1]])
# rotate vectors based on the permutation sequence
best_sequence = mappings_scores[0][2][0][1]
rotated_manipulation_vec = {}
for operation, vec in aabbs[mappings_scores[0]
[0]].manipulation_vectors.items():
if vec is not None:
vpython_vec = vpython.vec(vec[0], vec[1], vec[2])
# best_sequence not reversed because we want to rotate the manipulation vectors from
# source scenario to target one. Best sequence is from target to source.
# For example (x,y) means first transform y and then x.
# Since we want to go backwards it is already in the correct order.
for rotation in best_sequence:
if rotation == 'x':
vpython_vec = vpython.rotate(
vpython_vec,
angle=vpython.radians(90),
axis=vpython.vec(1, 0, 0))
elif rotation == 'y':
vpython_vec = vpython.rotate(
vpython_vec,
angle=vpython.radians(90),
axis=vpython.vec(0, 1, 0))
rotated_manipulation_vec[operation] = [
vpython_vec.x, vpython_vec.y, vpython_vec.z
]
else:
rotated_manipulation_vec[operation] = [None, None, None]
picked_obj.aabb.manipulation_vectors = rotated_manipulation_vec
# get dict that maps surfaces from target to source
# For example 'top':'left' top surface is going to be left to map to source
mapping_sides = analogy_mapping.all_permutations[best_sequence]
aabb_half_size = aabbs[mappings_scores[0][0]].half_size
aabb_pos = aabbs[mappings_scores[0][0]].pos
# determine x,y,z ratios for scaling factor that is applied
# for manipulation points positions. It is important to determine which
# rotation took place in order to determine correct scaling ratios.
x_ratio = 0
y_ratio = 0
z_ratio = 0
if (mapping_sides['top'] == 'top' or mapping_sides['top'] == 'bottom') and (
mapping_sides['front'] == 'front' or
mapping_sides['front'] == 'back'):
x_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[2]
elif (mapping_sides['top'] == 'top' or mapping_sides['top'] == 'bottom'
) and (mapping_sides['front'] == 'right' or
mapping_sides['front'] == 'left'):
x_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[2]
elif (mapping_sides['top'] == 'left' or mapping_sides['top'] == 'right'
) and (mapping_sides['front'] == 'front' or
mapping_sides['front'] == 'back'):
x_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[2]
elif (mapping_sides['top'] == 'left' or mapping_sides['top'] == 'right'
) and (mapping_sides['front'] == 'top' or
mapping_sides['front'] == 'bottom'):
x_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[2]
elif (mapping_sides['top'] == 'front' or mapping_sides['top'] == 'back'
) and (mapping_sides['front'] == 'bottom' or
mapping_sides['front'] == 'top'):
x_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[2]
elif (mapping_sides['top'] == 'front' or mapping_sides['top'] == 'back'
) and (mapping_sides['front'] == 'right' or
mapping_sides['front'] == 'left'):
x_ratio = picked_obj.aabb.half_size[2] / aabb_half_size[0]
y_ratio = picked_obj.aabb.half_size[0] / aabb_half_size[1]
z_ratio = picked_obj.aabb.half_size[1] / aabb_half_size[2]
else:
raise ValueError
rotated_manipulation_points = {}
# iterate through all manipulation points and rotate them based on the best sequence
for operation, pos in aabbs[mappings_scores[0]
[0]].manipulation_points.items():
if pos is not None:
# calculate relative position from manipulation point to centre of AABB
relative_pos = [
pos[0] - aabb_pos[0], pos[1] - aabb_pos[1], pos[2] - aabb_pos[2]
]
# Scale the position to match the size of our target AABB
scaled_relative_pos = [
relative_pos[0] * x_ratio, relative_pos[1] * y_ratio,
relative_pos[2] * z_ratio
]
# create vpython vec that is used in rotation method
vpython_vec = vpython.vec(scaled_relative_pos[0],
scaled_relative_pos[1],
scaled_relative_pos[2])
# rotate the manipulation points positions from source to target scene
for rotation in best_sequence:
if rotation == 'x':
vpython_vec = vpython.rotate(
vpython_vec,
angle=vpython.radians(90),
axis=vpython.vec(1, 0, 0))
elif rotation == 'y':
vpython_vec = vpython.rotate(
vpython_vec,
angle=vpython.radians(90),
axis=vpython.vec(0, 1, 0))
# Shift the manipulation point position from relative pos to absolute pos in the scene
scaled__rotated_abs_pos = [
vpython_vec.x + picked_obj.aabb.pos[0],
vpython_vec.y + picked_obj.aabb.pos[1],
vpython_vec.z + picked_obj.aabb.pos[2]
]
rotated_manipulation_points[operation] = scaled__rotated_abs_pos
# Draw rotated manipulation points and vectors on the screen
radius = stat.mean(picked_obj.aabb.half_size) / 10
if operation == 'push':
color = vpython.color.cyan
elif operation == 'pull':
color = vpython.color.purple
else:
color = vpython.color.orange
vpython_drawings.draw_point(
rotated_manipulation_points[operation], radius, color=color)
vector_length = stat.mean(picked_obj.aabb.half_size)
vpython_drawings.draw_arrow(
rotated_manipulation_points[operation],
picked_obj.aabb.manipulation_vectors[operation],
vector_length,
color=color)
else:
rotated_manipulation_points[operation] = [None, None, None]
# assig the calculated manipulation points to the target AABB
picked_obj.aabb.manipulation_points = rotated_manipulation_points
# save it to the DB if it is correct.
correct_result = input(
'Are these manipulation points and vectors correct? Yes/No: ')
if correct_result.lower() == 'y' or correct_result.lower() == 'yes':
file_path_list = obj_file_path.split('/')
file_name_obj = file_path_list[-1]
file_name_list = file_name_obj.split('.')
scene_name = file_name_list[0]
db.save_scene(scene, scene_name, picked_obj)
print('Successfully Saved in knowledge base DB.')
else:
print('This is not going to be saved in the knowledge base DB.')
print('Done')
# close DB
db.conn.close()
import math
import array
import numpy as np
import vpython as vp # pip install vpython
v1 = vp.vector(1, 0, 0) # creates a vector of floating points [1.0,0.0,0.0]
# # print(v1)
# # print(type(v1)) # class 'vpython.cyvector.vector'
# # print(v1.x, v1.y, v1.z) # returns each component
#
# # ROTATE
# degrees_from_vertical = 20.0
degrees_rotate = 90.0
theta = math.radians(degrees_rotate) # theta is radians of measured degrees
v2 = vp.rotate(v1, angle=theta, axis=vp.vector(0, 1, 0))
print("vp rotate: {}".format(v2))
# print(type(v2))
# print(v2) # outputs a rotation of v1 about the y-axis by theta radians
#
# # making own CS transform of 20 deg from vertical = 110 deg roration
# A_jake = np.array([[math.cos(math.radians(110)),math.cos(math.radians(0.0)),math.cos(math.radians(200.0))],
# [0,math.cos(math.radians(0)),0],
# [math.cos(math.radians(20)),0,math.cos(math.radians(110))]])
# # coordiate rotation matrix of 110 deg counter clockwise about y axis
# # print(A_jake)
# v_prime_jake = np.array([1,0,0]) # vector in prime CS aligned with x' axis
# v_jake = np.matmul(A_jake,v_prime_jake)
# print("v_jake rotate: {}".format(v_jake))
def moveView(self, event):
camAxis = self.scene.camera.axis
camDist = vp.mag(self.scene.center - self.scene.camera.pos)
dtheta = self.sensitivity
up = self.scene.up
if event.key in ["up", "k", "K"]:
self.scene.camera.pos -= up.norm() * dtheta * camDist
return
if event.key in ["down", "j", "J"]:
self.scene.camera.pos += up.norm() * dtheta * camDist
return
if event.key in ["right", "l", "L"]:
self.scene.camera.pos += vp.norm(
up.cross(camAxis)) * dtheta * camDist
return
if event.key in ["left", "h", "H"]:
self.scene.camera.pos -= vp.norm(
up.cross(camAxis)) * dtheta * camDist
return
if event.key in [".", ">"]: # Get closer, by ratio
ctr = self.scene.center
self.scene.camera.pos = ctr - camAxis / (1 + dtheta)
self.scene.camera.axis = ctr - self.scene.camera.pos
return
if event.key in [",", "<"]: # Get further
ctr = self.scene.center
self.scene.camera.pos = ctr - camAxis * (1 + dtheta)
self.scene.camera.axis = ctr - self.scene.camera.pos
return
if event.key == "p": # pitch: Rotate camera around ctr-horiz axis
self.scene.forward = vp.rotate(self.scene.forward,
angle=dtheta,
axis=vp.cross(
self.scene.forward,
self.scene.up))
return
if event.key == "P":
self.scene.forward = vp.rotate(self.scene.forward,
angle=-dtheta,
axis=vp.cross(
self.scene.forward,
self.scene.up))
return
if event.key == "y": # yaw: Rotate camera around ctr - up axis.
self.scene.forward = vp.rotate(self.scene.forward,
angle=dtheta,
axis=self.scene.up)
return
return
if event.key == "Y":
self.scene.forward = vp.rotate(self.scene.forward,
angle=-dtheta,
axis=self.scene.up)
return
if event.key == "r": # Roll, that is, change the 'up' vector
self.scene.camera.rotate(angle=dtheta,
axis=camAxis,
origin=self.scene.camera.pos)
return
if event.key == "R":
self.scene.camera.rotate(angle=-dtheta,
axis=camAxis,
origin=self.scene.camera.pos)
return
if event.key == "d": # Diameter scaling down
for dbl in self.drawables_:
dbl.diaScale *= 1.0 - self.sensitivity * 4
dbl.updateDiameter()
return
if event.key == "D":
for dbl in self.drawables_:
dbl.diaScale *= 1.0 + self.sensitivity * 4
dbl.updateDiameter()
return
if event.key == "s": # Scale down sleep time, make it faster.
self.sleep *= 1 - self.sensitivity
return
if event.key == "S": # Scale up sleep time, make it slower.
self.sleep *= 1 + self.sensitivity
return
if event.key == "a": # autoscale to fill view.
self.doAutoscale()
return
if event.key == "g":
self.hideAxis = not self.hideAxis
# show/hide the axis here.
if event.key == "t": # Turn on/off twisting/autorotate
self.doRotation = not self.doRotation
if event.key == "?": # Print out help for these commands
self.printMoogulHelp()
def getedadelays(offsets=EDAOFFSETS, az=0.0, el=90.0):
"""Create and return 3D objects representing the pointing delays for all 256 EDA dipoles.
They are represented by arrows for each dipole, with a length equal to that dipoles delay value
times the speed of light, and all point at the specified az/el direction. Positive delays
are represented by arrows below the ground plane, negative delays are represented by arrows
the ground plane.
Two sets of delays are shown - the ideal gemetric delays are shown in white, and the actual
integer delays (the sum of the first and second stage delays) are shown in green.
A tuple of (parrow, ilist, alist) is returned, where 'parrow' is a single, long, yellow arrow
from the centre of the EDA showing the pointing direction, ilist is the list of 'ideal'
delay arrows, and 'alist' is the list of 'actual' delay arrows. They are returned
separately, so the 'visible' attribute on each set of arrows can be set as needed.
"""
if ONLYBFs is not None:
clipdelays = False
else:
clipdelays = True
idelays, diagnostics = pointing.calc_delays(offsets=offsets,
az=az,
el=el,
strict=STRICT,
verbose=True,
clipdelays=clipdelays,
cpos=CPOS)
if diagnostics is not None:
delays, delayerrs, sqe, maxerr, offcount = diagnostics
if offcount > 0:
print(
'Elevation low - %d dipoles disabled because delays were too large to reach in hardware.'
% offcount)
else:
delays, delayerrs, sqe, maxerr, offcount = None, None, None, None, None
if idelays is None:
print("Error calculating delays for az=%s, el=%s" % (az, el))
return []
if ONLYBFs is None:
offcount = 0
for bfid in pointing.HEXD:
for dipid in pointing.HEXD:
if idelays[bfid][dipid] == 16: # disabled
offcount += 1
elif len(
ONLYBFs
) == 1: # We only have one beamformer enabled, so disable all other dipoles and normalise remaining to the minimum delay on that BF
offcount = 240
print(
"Only one first stage beamformer enabled (%s) and delays normalised to the minimum value, other 240 dipoles are disabled!"
% ONLYBFs)
for bfid in pointing.HEXD:
if bfid in ONLYBFs: # If this is one of the beamformer we want to point:
mind = min(idelays[bfid].values()
) # Find the smallest delay in this BF
for dipid in pointing.HEXD:
idelays[bfid][dipid] -= (
mind + 16
) # Subtract the minimum delay from the given delay, then subtract 16
if (idelays[bfid][dipid] < -16) or (idelays[bfid][dipid] >
15):
idelays[bfid][dipid] = 16 # Disabled
offcount += 1
else:
for dipid in pointing.HEXD:
idelays[bfid][
dipid] = 16 # Disable all dipoles on other beamformers
else:
offcount = 0
print(
"Only some first stage beamformer enabled (%s), other %d dipoles are disabled!"
% (ONLYBFs, offcount))
for bfid in pointing.HEXD:
for dipid in pointing.HEXD:
if bfid in ONLYBFs:
if (idelays[bfid][dipid] < -16) or (idelays[bfid][dipid] >
15):
idelays[bfid][dipid] = 16 # Disabled
offcount += 1
else:
idelays[bfid][dipid] = 16 # Disabled
offcount += 1
north = v(0, 1, 0) # Due north, elevation 0 degrees
t1 = vpython.rotate(north, angle=(el * math.pi / 180.0), axis=v(
1, 0, 0)) # Rotate up (around E/W axis) by 'el' degrees
pvector = vpython.rotate(
t1, angle=(-az * math.pi / 180.0),
axis=v(0, 0, 1)) # Rotate clockwise by 'az' degrees around 'up' axis
parrow = vpython.arrow(pos=v(0, 0, 0),
axis=pvector,
color=color.yellow,
length=20.0,
shaftwidth=1.0,
visible=True)
ilist = []
alist = []
dlist = []
for bfid in pointing.HEXD:
for dipid in pointing.HEXD:
# Arrow lengths are negative if delays are positive, and vice/versa
if delays:
idealdelay = delays[bfid][dipid] * pointing.C
ilist.append(
vpython.arrow(pos=v(*offsets[bfid][dipid]),
axis=pvector,
length=-idealdelay,
color=color.white,
shaftwidth=0.2,
visible=ivis))
if idelays[bfid][dipid] != 16: # If this dipole isn't disabled
actualdelay = (
(idelays[bfid][dipid] * pointing.MSTEP) +
(idelays['K'][bfid] * pointing.KSTEP)) * pointing.C
alist.append(
vpython.arrow(pos=v(*offsets[bfid][dipid]),
axis=pvector,
length=-actualdelay,
color=color.green,
shaftwidth=0.2,
visible=avis))
delaydifference = idealdelay - actualdelay
dlist.append(
vpython.arrow(pos=v(*offsets[bfid][dipid]),
axis=pvector,
length=100 * delaydifference,
color=color.red,
shaftwidth=0.2,
visible=dvis))
return parrow, ilist, alist, dlist
def rotateAntiClockwise(array):
return rotate(array, 90)
moon.sidperiod = 27.321661 # Sidereal Period (moon's 'year', in Earth days)
moon.rotperiod = 27.321661 # Length of the planets sidereal day, in Earth days
marrow = vpython.arrow(pos=moon.pos, axis=v(-1, 0, 0))
# tlabel=label(pos=v(7,7,0), text='', xoffset=0, yoffset=0, box=0)
stheta = 0.0
rtheta = 0.0
dt = 0.01
t = 0.0
time.sleep(5) # Pause for a bit before starting
while 1: # Do the animation
vpython.rate(100)
t = t + dt
moon.pos = vpython.rotate(startpos, angle=stheta)
moon.pos.mag = moon.a * (1 - moon.e * moon.e) / (1 +
moon.e * math.cos(stheta))
marrow.pos = moon.pos
marrow.axis = vpython.rotate(v(-1, 0, 0), angle=rtheta)
# tlabel.text="Time (days) %06.2f" % t
stheta = stheta + (dt / moon.sidperiod) * 2 * math.pi
rtheta = rtheta + (dt / moon.rotperiod) * 2 * math.pi
if stheta > 2 * math.pi:
stheta = stheta - 2 * math.pi
if rtheta > 2 * math.pi:
rtheta = rtheta - 2 * math.pi
def get_mappings_score(self, target_aabb, source_aabb):
"""
Returns analogy scores for all mappings of two AABB objects.
Args:
target_aabb(AABB): The target AABB that we want to map to source AABB.
source_aabb(AABB): The source AABB that from source scene.
Returns:
Analogy scores for all mappings (sequence permutations) of two AABB
objects based on collided sides simmilarity, ratio of AABBs,
matching surfaces.
"""
# collision match weights
exact_collision_weight = 0.99 #0.9
partial_collision_weight = 0.5 #0.4
no_collision_match_weight = .0 #-0.1
# surface match weights
top_match_weight = .02 #0.1
bottom_match_weight = .02 #0.1
front_match_weight = .01 #0.01
back_match_weight = .01 #0.01
right_match_weight = .01 #0.01
left_match_weight = .01 #0.01
no_surf_match_weight = .0 #-0.01
# surface ratio difference penalties
xy_ratio_weight = 0.1
zy_ratio_weight = 0.1
xz_ratio_weight = 0.1
mappings_score = []
for perm_sequence in self.all_permutations.keys():
score = 1.0 # default score
for surface in self.all_permutations[perm_sequence].keys():
# scores for exact collision/partial/no-collision match
if target_aabb.collided_sides[
surface] == source_aabb.collided_sides[
self.all_permutations[perm_sequence][surface]]:
score += 1.0 * exact_collision_weight
# match partially collided sides and fully collided sides
elif target_aabb.collided_sides[
surface] != 0 and source_aabb.collided_sides[
self.all_permutations[perm_sequence]
[surface]] != 0:
score += 1.0 * partial_collision_weight
else: # there is no match
score += 1.0 * no_collision_match_weight
# score for surface match
if (surface == 'top'
and self.all_permutations[perm_sequence][surface]
== 'top'):
score += 1.0 * top_match_weight
elif (surface == 'bottom'
and self.all_permutations[perm_sequence][surface]
== 'bottom'):
score += 1.0 * bottom_match_weight
elif (surface == 'front'
and self.all_permutations[perm_sequence][surface]
== 'front'):
score += 1.0 * front_match_weight
elif (surface == 'back' and
self.all_permutations[perm_sequence][surface] == 'back'):
score += 1.0 * back_match_weight
elif (surface == 'right'
and self.all_permutations[perm_sequence][surface]
== 'right'):
score += 1.0 * right_match_weight
elif (surface == 'left' and
self.all_permutations[perm_sequence][surface] == 'left'):
score += 1.0 * left_match_weight
else:
score += 1.0 * no_surf_match_weight
# create vpython vec for xyz half size of the target object
vpython_vec_xyz = vpython.vec(target_aabb.half_size[0],
target_aabb.half_size[1],
target_aabb.half_size[2])
# rotate it based on rotation sequence
for rotation in reversed(perm_sequence):
if rotation == 'x':
vpython_vec_xyz = vpython.rotate(
vpython_vec_xyz,
angle=vpython.radians(-90),
axis=vpython.vec(1, 0, 0))
elif rotation == 'y':
vpython_vec_xyz = vpython.rotate(
vpython_vec_xyz,
angle=vpython.radians(-90),
axis=vpython.vec(0, 1, 0))
# compare xy, zy, xz ratios of target and source aabbs after rotation
# and penalise for difference
xy_ratio_diff = abs(
(abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.y)) -
(source_aabb.half_size[0] / source_aabb.half_size[1]))
xy_ratio_diff *= xy_ratio_weight
zy_ratio_diff = abs(
(abs(vpython_vec_xyz.z) / abs(vpython_vec_xyz.y)) -
(source_aabb.half_size[2] / source_aabb.half_size[1]))
zy_ratio_diff *= zy_ratio_weight
xz_ratio_diff = abs(
(abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.z)) -
(source_aabb.half_size[0] / source_aabb.half_size[2]))
xz_ratio_diff *= xz_ratio_weight
sum_ratio_diff = xy_ratio_diff + zy_ratio_diff + xz_ratio_diff
# penalise for ratio difference
score -= sum_ratio_diff
mappings_score.append((score, perm_sequence))
return mappings_score
F_SE = G * MS * ME / (RSE * RSE)
# Гравитационная сила между Землей и Луной, Н
F_EM = G * ME * MM / (REM * REM)
# Угловая скорость Луны
wm = math.sqrt(F_EM / (MM * REM))
# Угловая скорость Земли
we = math.sqrt(F_SE / (ME * RSE))
v = vector(0.5, 0, 0)
Earth = sphere(pos=vector(3, 0, 0),
color=color.blue,
radius=.25,
make_trail=True)
Moon = sphere(pos=Earth.pos + v,
color=color.white,
radius=0.08,
make_trail=True)
Sun = sphere(pos=vector(0, 0, 0), color=color.yellow, radius=1)
# Будем использовать полярные координаты
# Шаг
dt = 10
# углы поворота за один шаг:
theta_earth = we * dt
theta_moon = wm * dt
while dt <= 86400 * 365:
# Земля и Луна поворачиваются вокруг оси z (0,0,1)
Earth.pos = rotate(Earth.pos, angle=theta_earth, axis=vector(0, 0, 1))
v = rotate(v, angle=theta_moon, axis=vector(0, 0, 1))
Moon.pos = Earth.pos + v
dt += 10
def gettiledelays(cpos=None, az=0.0, el=90.0):
"""
Copied from function calc_delays in obssched/pycontroller.py, with
code to create the actual arrow objects added. If cpos is given, it's
used as the tile centre position - use this when showing more than one
tile.
Algorithm copied from ObsController.java and converted to Python
This function takes in an azimuth and zenith angle as
inputs and creates and returns a 16-element byte array for
delayswitches which have values corresponding to each
dipole in the tile having a maximal coherent amplitude in the
desired direction.
This will return null if the inputs are
out of physical range (if za is bigger than 90) or
if the calculated switches for the dipoles are out
of range of the delaylines in the beamformer.
azimuth of 0 is north and it increases clockwise
zenith angle is the angle down from zenith
These angles should be given in degrees
Layout of the dipoles on the tile:
N
0 1 2 3
4 5 6 7
W E
8 9 10 11
12 13 14 15
S
"""
if cpos is None:
cpos = v(0, 0, 0)
elif type(cpos) == tuple:
cpos = v(cpos)
# Find the delay values for the nearest sweetspot, to use for green arrows:
sweetaz, sweetel, sweetdelays = get_sweet_delays(az=az, el=el)
# Calculate the geometric delays for the ax/el given, without using sweetspot
dip_sep = 1.10 # dipole separations in meters
delaystep = 435.0 # Delay line increment in picoseconds
maxdelay = 31 # Maximum number of deltastep delays
c = 0.000299798 # C in meters/picosecond
dtor = math.pi / 180.0 # convert degrees to radians
# define zenith angle
za = 90 - el
# Define arrays to hold the positional offsets of the dipoles
xoffsets = [0.0] * 16 # offsets of the dipoles in the W-E 'x' direction
yoffsets = [0.0] * 16 # offsets of the dipoles in the S-N 'y' direction
delays = [0.0] * 16 # The calculated delays in picoseconds
rdelays = [0] * 16 # The rounded delays in units of delaystep
delaysettings = [0] * 16 # return values
# Check input sanity
if (abs(za) > 90):
return None
# Offsets of the dipoles are calculated relative to the
# center of the tile, with positive values being in the north
# and east directions
xoffsets[0] = -1.5 * dip_sep
xoffsets[1] = -0.5 * dip_sep
xoffsets[2] = 0.5 * dip_sep
xoffsets[3] = 1.5 * dip_sep
xoffsets[4] = -1.5 * dip_sep
xoffsets[5] = -0.5 * dip_sep
xoffsets[6] = 0.5 * dip_sep
xoffsets[7] = 1.5 * dip_sep
xoffsets[8] = -1.5 * dip_sep
xoffsets[9] = -0.5 * dip_sep
xoffsets[10] = 0.5 * dip_sep
xoffsets[11] = 1.5 * dip_sep
xoffsets[12] = -1.5 * dip_sep
xoffsets[13] = -0.5 * dip_sep
xoffsets[14] = 0.5 * dip_sep
xoffsets[15] = 1.5 * dip_sep
yoffsets[0] = 1.5 * dip_sep
yoffsets[1] = 1.5 * dip_sep
yoffsets[2] = 1.5 * dip_sep
yoffsets[3] = 1.5 * dip_sep
yoffsets[4] = 0.5 * dip_sep
yoffsets[5] = 0.5 * dip_sep
yoffsets[6] = 0.5 * dip_sep
yoffsets[7] = 0.5 * dip_sep
yoffsets[8] = -0.5 * dip_sep
yoffsets[9] = -0.5 * dip_sep
yoffsets[10] = -0.5 * dip_sep
yoffsets[11] = -0.5 * dip_sep
yoffsets[12] = -1.5 * dip_sep
yoffsets[13] = -1.5 * dip_sep
yoffsets[14] = -1.5 * dip_sep
yoffsets[15] = -1.5 * dip_sep
# First, figure out the theoretical delays to the dipoles
# relative to the center of the tile
# Convert to radians
azr = az * dtor
zar = za * dtor
for i in range(16):
# calculate exact delays in picoseconds from geometry...
delays[i] = (xoffsets[i] * math.sin(azr) +
yoffsets[i] * math.cos(azr)) * math.sin(zar) / c
# Find minimum delay
mindelay = min(delays)
# Subtract minimum delay so that all delays are positive
for i in range(16):
delays[i] -= mindelay
# Now minimize the sum of the deviations^2 from optimal
# due to errors introduced when rounding the delays.
# This is done by stepping through a series of offsets to
# see how the sum of square deviations changes
# and then selecting the delays corresponding to the min sq dev.
# Go through once to get baseline values to compare
bestoffset = -0.45 * delaystep
minsqdev = 0
for i in range(16):
delay_off = delays[i] + bestoffset
intdel = int(round(delay_off / delaystep))
if (intdel > maxdelay):
intdel = maxdelay
minsqdev += math.pow((intdel * delaystep - delay_off), 2)
minsqdev = minsqdev / 16
offset = (-0.45 * delaystep) + (delaystep / 20.0)
while offset <= (0.45 * delaystep):
sqdev = 0
for i in range(16):
delay_off = delays[i] + offset
intdel = int(round(delay_off / delaystep))
if (intdel > maxdelay):
intdel = maxdelay
sqdev = sqdev + math.pow((intdel * delaystep - delay_off), 2)
sqdev = sqdev / 16
if (sqdev < minsqdev):
minsqdev = sqdev
bestoffset = offset
offset += delaystep / 20.0
for i in range(16):
rdelays[i] = int(round((delays[i] + bestoffset) / delaystep))
if (rdelays[i] > maxdelay):
if (rdelays[i] > maxdelay + 1):
return None # Trying to steer out of range.
rdelays[i] = maxdelay
# Set the actual delays
for i in range(16):
delaysettings[i] = int(rdelays[i])
if mode == 'EDA':
parrowlen = 20.0
parrowsw = 1.0
else:
parrowlen = 5.0
parrowsw = 0.2
north = v(0, 1, 0) # Due north, elevation 0 degrees
at1 = vpython.rotate(north, angle=(el * math.pi / 180), axis=v(
1, 0, 0)) # Rotate up (around E/W axis) by 'el' degrees
apvector = vpython.rotate(
at1, angle=(-az * math.pi / 180),
axis=v(0, 0, 1)) # Rotate clockwise by 'az' degrees around 'up' axis
aarrow = vpython.arrow(pos=v(0, 0, 0),
axis=apvector,
color=color.white,
length=parrowlen,
shaftwidth=parrowsw,
visible=avis)
if (sweetaz is not None) and (sweetel is not None):
st1 = vpython.rotate(
north, angle=(sweetel * math.pi / 180),
axis=v(1, 0, 0)) # Rotate up (around E/W axis) by 'el' degrees
spvector = vpython.rotate(
st1, angle=(-sweetaz * math.pi / 180),
axis=v(0, 0,
1)) # Rotate clockwise by 'az' degrees around 'up' axis
sarrow = vpython.arrow(pos=v(0, 0, 0),
axis=spvector,
color=color.green,
length=parrowlen,
shaftwidth=parrowsw,
visible=avis)
alist = [sarrow]
else:
alist = []
spvector = None
ilist = [aarrow]
dlist = []
for i in range(16):
# Arrow lengths are negative if delays are positive, and vice/versa
idealdelay = delaysettings[i] * TILEDELAYSTEP * pointing.C
dposx, dposy = TILEOFFSETS[i]
dpos = v(dposx, dposy, 0) + cpos
if sweetdelays:
sweetdelay = sweetdelays[i] * TILEDELAYSTEP * pointing.C
alist.append(
vpython.arrow(pos=dpos,
axis=spvector,
length=-sweetdelay,
color=color.green,
shaftwidth=0.2,
visible=avis))
ilist.append(
vpython.arrow(pos=dpos,
axis=apvector,
length=-idealdelay,
color=color.white,
shaftwidth=0.2,
visible=avis))
# Tiles have two pointing arrows, stored in the 'alist' and 'ilist', so return None for the first element.
return None, ilist, alist, dlist
autoscale = 0
autocenter = 0
estheta = 0.0
ertheta = 0.0
mstheta = 0.0
mrtheta = 0.0
dt = 1.0
t = 0.0
time.sleep(1) # Pause for a bit before starting
while 1: # Do the animation
vpython.rate(100)
t = t + dt
earth.pos = vpython.rotate(earthstart, angle=estheta)
earth.pos.mag = earth.a * (1 - earth.e * earth.e) / (1 + earth.e * math.cos(estheta))
mars.pos = vpython.rotate(marsstart, angle=mstheta)
mars.pos.mag = mars.a * (1 - mars.e * mars.e) / (1 + mars.e * math.cos(mstheta))
tlabel.text = "Time %06.2f (days) = %6.4f (years) " % (t, t / 365.246)
estheta = estheta + (dt / earth.sidperiod) * 2 * math.pi
if estheta > 2 * math.pi:
estheta = estheta - 2 * math.pi
mstheta = mstheta + (dt / mars.sidperiod) * 2 * math.pi
if mstheta > 2 * math.pi:
mstheta = mstheta - 2 * math.pi
