def constraints(self, particle):
# adjust the effects of the limits (constraints) of the world on the partcle
# it must run before acceleration and velocity are applied in impulse
World.Log_Data("constraint start: {}".format(particle))
if particle.position.get_z() > self.max_z:
particle.velocity = Vector3dm.zero_vector()
particle.acceleration = Vector3dm.zero_vector()
particle.position.set_z(self.max_z)
World.Log_Data(
"YES! Rock bottom post constraints:{}".format(particle))
if particle.position.get_y() > self.max_y:
particle.position.set_y(self.max_y)
if particle.position.get_x() > self.max_x:
particle.position.set_y(self.max_x)
if particle.position.get_y() < self.min_y:
particle.position.set_y(self.min_y)
if particle.position.get_x() < self.min_x:
particle.position.set_x(self.min_x)
if particle.position.get_z() < self.min_z:
particle.position.set_z(self.min_z)
if particle.velocity.get_z() > 20.0:
particle.velocity.set_z(20.0)
World.Log_Data("constraint end: {}".format(particle))
def test_create_particle(self):
p = Particle(Vector3dm(1, 2, 3, "c"), Vector3dm(4, 5, 6, "c"))
self.assertAlmostEqual(
p.position.get_x(),
1), "particle init: x pos bad result: is {} should be {}".format(
p.position.get_x(), 1)
self.assertAlmostEqual(
p.position.get_y(),
2), "particle init: y pos bad result: is {} should be {}".format(
p.position.get_y(), 2)
self.assertAlmostEqual(
p.position.get_z(),
3), "particle init: z pos bad result: is {} should be {}".format(
p.position.get_z(), 3)
self.assertAlmostEqual(
p.velocity.get_x(),
4), "particle init: x vel bad result: is {} should be {}".format(
p.position.get_x(), 4)
self.assertAlmostEqual(
p.velocity.get_y(),
5), "particle init: y vel bad result: is {} should be {}".format(
p.position.get_y(), 5)
self.assertAlmostEqual(
p.velocity.get_z(),
6), "particle init: z vel bad result: is {} should be {}".format(
p.position.get_z(), 6)
def test_add(self):
expected_sum_x = -6
expected_sum_y = 13
expected_sum_z = -25
v1 = Vector3dm(13,-5,-20,"c")
v2 = Vector3dm(-19,18,-5,"c")
v3 = v1.add(v2)
x,y,z = v3.vals
self.assertAlmostEqual(x,expected_sum_x,6,"origindist bad distance: is {} should be {}".format(x,expected_sum_x))
self.assertAlmostEqual(y,expected_sum_y,6,"origindist bad distance: is {} should be {}".format(y,expected_sum_y))
self.assertAlmostEqual(z,expected_sum_z,6,"origindist bad distance: is {} should be {}".format(z,expected_sum_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
self.assertEqual(type(v3.vals),type([]),"test changed vals type")
def test_sub(self):
expected_sum_x = 10
expected_sum_y = -21
expected_sum_z = 9
v1 = Vector3dm(-9,-4,10,"c")
v2 = Vector3dm(-19,17,1,"c")
v3 = v1.sub(v2)
x,y,z = v3.vals
self.assertAlmostEqual(x,expected_sum_x,6,"origindist bad distance: is {} should be {}".format(x,expected_sum_x))
self.assertAlmostEqual(y,expected_sum_y,6,"origindist bad distance: is {} should be {}".format(y,expected_sum_y))
self.assertAlmostEqual(z,expected_sum_z,6,"origindist bad distance: is {} should be {}".format(z,expected_sum_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
self.assertEqual(type(v3.vals),type([]),"test changed vals type")
def test_inner(self):
expected_dot = -14.0
x1 = -1
y1 = 2
z1 = -3
x2 = 1
y2 = -2
z2 = 3
v1 = Vector3dm(x1,y1,z1,"c")
v2 = Vector3dm(x2,y2,z2,"c")
dot = v1.dot(v2)
self.assertAlmostEqual(dot,expected_dot,6,"Dot bad result: is {} should be {}".format(dot,expected_dot))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def test_dot(self):
# got example from https://chortle.ccsu.edu/VectorLessons/vch07/vch07_14.html
expected_dot = -14.0
x1 = -1
y1 = 2
z1 = -3
x2 = 1
y2 = -2
z2 = 3
v1 = Vector3dm(x1,y1,z1,"c")
v2 = Vector3dm(x2,y2,z2,"c")
dot = v1.dot(v2)
self.assertAlmostEqual(dot,expected_dot,6,"Dot bad result: is {} should be {}".format(dot,expected_dot))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def test_cross_product(self):
v1 = Vector3dm(2,3,4,"c")
v2 = Vector3dm(5,6,7,"c")
r_x = -3.0
r_y = 6.0
r_z = -3.0
v1_x_v2 = v1.cross(v2)
x = v1_x_v2.get_x()
y = v1_x_v2.get_y()
z = v1_x_v2.get_z()
self.assertAlmostEqual(x,r_x,6,"test_cross bad result: is {} should be {}".format(x,r_x))
self.assertAlmostEqual(y,r_y,6,"test_cross bad result: is {} should be {}".format(y,r_y))
self.assertAlmostEqual(z,r_z,6,"test_cross bad result: is {} should be {}".format(z,r_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def test_origin_distance(self):
expected_dist = 1.7320508075688772935274463415059
v1 = Vector3dm(-1,1,1,"c")
dist = v1.origin_distance()
self.assertAlmostEqual(dist,expected_dist,6,"origindist bad distance: is {} should be {}".format(dist,expected_dist))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
def __init__(self, pos_vect, vel_vect=Vector3dm.zero_vector(), id=""):
self.position = pos_vect
self.velocity = vel_vect
self.acceleration = Vector3dm.zero_vector()
self.id = id
self.count = 0
# various accelration Attributes
# aerodynamics
self.aero_coef = .05 # scale factor for Aerodynamic drag which opposes velocity
#colision avoidance
self.col_size = 1 # collision size
self.dis_size = 20 # display size
self.color = (255, 255, 255)
self.flash_color = (255, 50, 0) # bright red
def random_goal(self, current_pos, min_dist, max_dist):
# given your current position and how far (min and max) you want to go, this gets you a goal
# input: vector of current_pos, scaler of minimum and maximum distance
# return: a vector with the new position of the random goal.
# self.width = float(width) # total x dimension
# self.depth = float(depth) # total y dimension
# self.height = float(height) # total z dimension
# self.min_x = -width/2 # x coordinate of left wall
# self.min_y = -depth/2 # y coordinate of back wall
# self.min_z = 0.0 #z coordinate of floor
# self.max_x = self.width/2
# self.max_y = self.depth/2
# self.max_z = self.height
dist = random.random() * (max_dist - min_dist) + min_dist
theta = random.random() * 2 * math.pi
phi = random.random() * math.pi
result = Vector3dm(dist, theta, phi, "s").add(current_pos)
x, y, z = result.convert_to_cartesian().vals
if x > self.max_x:
result.set_x(self.max_x)
elif x < self.min_x:
result.set_x(self.min_x)
if y > self.max_y:
result.set_y(self.max_y)
elif y < self.min_y:
result.set_y(self.min_y)
if z > self.max_z:
result.set_z(self.max_z)
elif z < self.min_z:
result.set_z(self.min_z)
return result
def rand_vector(v, min_dist, max_dist):
min_x = -500
max_x = 500
min_y = -500
max_y = 500
min_z = 0
max_z = 1000
dist = random.random() * (max_dist - min_dist) + min_dist
theta = random.random() * 2 * math.pi
phi = random.random() * math.pi
old_v = Vector3dm(dist, theta, phi, "s").add(v)
v = old_v.convert_to_cartesian()
x, y, z = v.vals
#print(v,x,y,z)
#bad_vec = False
#if x>max_x or x<min_x or y>max_y or y<min_y or z>max_z or z<min_z:
# bad_vec = True
if x > max_x:
v.set_x(max_x)
elif x < min_x:
v.set_x(min_x)
if y > max_y:
v.set_y(max_y)
elif y < min_y:
v.set_y(min_y)
if z > max_z:
v.set_z(max_z)
elif z < min_z:
v.set_z(min_z)
#if bad_vec:
# print("bad vec",dist,old_v.convert_to_cartesian(),v)
return v
def create_world(self,particle_count): self.pixies = [] for i in range(0,particle_count): position = Vector3dm(-400,-400,800,"c") pixie = Pixie(position) self.particles.append(pixie)
def test_convert_to_spherical(self):
x = 2*math.sqrt(3)
y = 6
z = -4
v = Vector3dm(x,y,z,"c")
# the expected answer
tr = 8.0
ttheta = math.pi/3.0
tphi = 2*math.pi/3.0
v2 = v.convert_to_spherical()
r,theta,phi = v2.vals
self.assertAlmostEqual(r,tr,6,"c2s bad r: is {} should be {}".format(r,tr))
self.assertAlmostEqual(theta,ttheta,6,"c2s bad theta: is {} should be {}".format(theta,ttheta))
self.assertAlmostEqual(phi,tphi,6,"c2s bad phi: is {} should be {}".format(phi,tphi))
r,theta,phi = v2.convert_to_spherical().vals # should do nothing
self.assertAlmostEqual(r,tr,6,"c2s #2 bad r: is {} should be {}".format(r,tr))
self.assertAlmostEqual(theta,ttheta,6,"c2s #2 bad theta: is {} should be {}".format(theta,ttheta))
self.assertAlmostEqual(phi,tphi,6,"c2s bad #2 phi: is {} should be {}".format(phi,tphi))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def create_world(self, particle_count):
for i in range(0, particle_count):
position = Vector3dm(
random.randrange(int(self.min_x), int(self.max_x)),
random.randrange(int(self.min_y), int(self.max_y)),
random.randrange(int(self.min_z), int(self.max_z)), "c")
self.particles.append(Particle(pos_vect=position, id=i))
def test_get_z(self):
x = 1
y = 2
z = 3
v1 = Vector3dm(z,y,z,"c")
res_z = v1.get_z()
self.assertAlmostEqual(res_z,z,6,"get_z bad result: is {} should be {}".format(res_z,z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
def test_get_phi(self):
r = 1
theta = 2
phi = 3
v1 = Vector3dm(r,theta,phi,"s")
res_phi = v1.get_phi()
self.assertAlmostEqual(res_phi,phi,6,"get_phi bad result: is {} should be {}".format(res_phi,phi))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
def test_set_z(self):
x = 1
y = 2
z = 3
v = Vector3dm(x,y,z,"c")
v.set_z(10)
z = v.get_z()
self.assertAlmostEqual(10,z,6,"set_z bad result: is {} should be {}".format(z,10))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
def test_set_r(self):
r = 1
theta = 2
phi = 3
v = Vector3dm(r,theta,phi,"s")
v.set_r(10)
r = v.get_r()
self.assertAlmostEqual(10,r,6,"set_phi bad result: is {} should be {}".format(r,10))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
def test_unit_vector(self):
v = Vector3dm(12,-3,-4,"c").unit()
r_x = 12/13
r_y = -3/13
r_z = -4/13
x,y,z = v.get_x(),v.get_y(),v.get_z()
self.assertAlmostEqual(x,r_x,6,"test_unit bad result: is {} should be {}".format(x,r_x))
self.assertAlmostEqual(y,r_y,6,"test_unit bad result: is {} should be {}".format(y,r_y))
self.assertAlmostEqual(z,r_z,6,"test_unit bad result: is {} should be {}".format(z,r_z))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
def brain(self):
# this is code that changes the acceleration based on pixie desires.
result = Vector3dm.zero_vector()
CLOSE_RANGE = 10.0
MAX_ACC = 20.0
if self.goal is None:
self.goal = self.goals[self.goal_idx]
self.goal_idx += 1
if self.goal_idx > len(self.goals):
self.goal_idx = 0
World.Log_Data("brain New Goal: {} (#{})".format(self.goal,self.goal_idx))
return result
range = self.position.magnitude(self.goal)
if range < CLOSE_RANGE: # we've reached our goal
self.goal = None
World.Log_Data("brain REACHED goal")
return result
World.Log_Data("brain Current goal: {}".format(self.goal))
World.Log_Data("brain range: {}".format(range))
direction = self.position.point_at_that(self.goal)
World.Log_Data("point at {} from {} to {}".format(direction.convert_to_cartesian(),self.position,self.goal))
if direction.get_r() > MAX_ACC:
direction.set_r(MAX_ACC)
speed = self.velocity.get_r()
slowing = speed/MAX_ACC # Seconds to slow
arrival = range/speed # seconds to get there
if slowing < arrival:
result = direction
World.Log_Data("brain Speeding Toward Direction: {}={}".format(result,result.convert_to_cartesian()))
else:
if speed <= CLOSE_RANGE:
return Vector3dm.zero_vector() #coast closer
elif speed < CLOSE_RANGE+MAX_ACC:
direction.set_r(speed - CLOSE_RANGE) #get the speed to around the close_range; fastest we can go to not overshoot
result = direction.neg()
World.Log_Data("brain SLOWING toward direction: {} (Direction: {}".format(result,direction))
return result
def test_magnitude(self):
expected_mag = 18.78829423
v1 = Vector3dm(7,4,1,"c")
v2 = Vector3dm(13,18,-10,"c")
mag = v1.magnitude(v2)
self.assertAlmostEqual(mag,expected_mag,6,"magnitude1 bad mag: is {} should be {}".format(mag,expected_mag))
expected_mag = 29.06888371
v1 = Vector3dm(-3,18,6,"c")
v2 = Vector3dm(8,-2,-12,"c")
mag = v1.magnitude(v2)
self.assertAlmostEqual(mag,expected_mag,6,"magnitude2 bad mag: is {} should be {}".format(mag,expected_mag))
expected_mag = 23.53720459
v1 = Vector3dm(12,-19,7,"c")
mag = v1.magnitude()
self.assertAlmostEqual(mag,expected_mag,6,"magnitude3 bad mag: is {} should be {}".format(mag,expected_mag))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def test_conversion_inversion_c2s_s2c(self):
tx = 2*math.sqrt(3)
ty = 6
tz = -4
v = Vector3dm(tx,ty,tz,"c")
x,y,z = v.convert_to_spherical().convert_to_cartesian().vals
self.assertAlmostEqual(x,tx,6,"inversion1 bad x: is {} should be {}".format(x,tx))
self.assertAlmostEqual(y,ty,6,"inversion1 bad y: is {} should be {}".format(y,ty))
self.assertAlmostEqual(z,tz,6,"inversion1 bad z: is {} should be {}".format(z,tz))
tr = 8
ttheta = math.pi/4
tphi = math.pi/6
v = Vector3dm(tr,ttheta,tphi,"s")
r,theta,phi = v.convert_to_cartesian().convert_to_spherical().vals
self.assertAlmostEqual(r,tr,6,"inversion2 #2 bad r: is {} should be {}".format(r,tr))
self.assertAlmostEqual(theta,ttheta,6,"inversion2 #2 bad theta: is {} should be {}".format(theta,ttheta))
self.assertAlmostEqual(phi,tphi,6,"inversion2 bad #2 phi: is {} should be {}".format(phi,tphi))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
def test_zero_vector(self):
zero_vector = Vector3dm.zero_vector()
vx = zero_vector.get_x()
vy = zero_vector.get_y()
vz = zero_vector.get_z()
vr = zero_vector.get_r()
self.assertAlmostEqual(vx,0.0,6,"test_zero_vector bad result: is {} should be {}".format(vx,0.0))
self.assertAlmostEqual(vy,0.0,6,"test_zero_vector bad result: is {} should be {}".format(vy,0.0))
self.assertAlmostEqual(vz,0.0,6,"test_zero_vector bad result: is {} should be {}".format(vz,0.0))
self.assertAlmostEqual(vr,0.0,6,"test_zero_vector bad result: is {} should be {}".format(vr,0.0))
self.assertEqual(type(zero_vector.vals),type([]),"test changed vals type")
def test_random_goal(self):
w = World(1, 40, 40, 40)
pos = Vector3dm(0, 0, 0, "c")
goal = w.random_goal(pos, 100, 500)
x, y, z = goal.convert_to_cartesian().vals
self.assertLessEqual(x, 20)
self.assertLessEqual(y, 20)
self.assertLessEqual(z, 40)
self.assertGreaterEqual(x, -20)
self.assertGreaterEqual(y, -20)
self.assertGreaterEqual(z, 0)
def test_mult(self):
v = Vector3dm(1,1,1,"c")
v_mult = v.mult(6.0)
x,y,z = v_mult.vals
self.assertAlmostEqual(x,6.0,6,"mult x: is {} should be {}".format(x,6.0))
self.assertAlmostEqual(y,6.0,6,"mult y: is {} should be {}".format(y,6.0))
self.assertAlmostEqual(z,6.0,6,"mult z: is {} should be {}".format(z,6.0))
self.assertEqual(type(v.vals),type([]),"test changed vals type")
r = v_mult.get_r()
self.assertAlmostEqual(r, 10.392304845413264,6,"mult r: is {} should be {}".format(r,6.0))
def test_neg(self):
expected_sum_x = 5
expected_sum_y = 13
expected_sum_z = -4
v1 = Vector3dm(-5,-13,4,"c")
v3 = v1.neg()
x,y,z = v3.vals
self.assertAlmostEqual(x,expected_sum_x,6,"origindist bad distance: is {} should be {}".format(x,expected_sum_x))
self.assertAlmostEqual(y,expected_sum_y,6,"origindist bad distance: is {} should be {}".format(y,expected_sum_y))
self.assertAlmostEqual(z,expected_sum_z,6,"origindist bad distance: is {} should be {}".format(z,expected_sum_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v3.vals),type([]),"test changed vals type")
def gravity(self, particle):
# return the value for gravity in m/impulse/impulse
#gravity_acc = particle.position.point_at_that(self.gravity_locus)
#World.Log_Data("point at:{}".format(gravity_acc.convert_to_cartesian()))
#World.Log_Data("self.gravity_mag: {}".format(self.gravity_mag* float(self.impulse_duration) / World.MICROS_IN_SEC))
#gravity_acc.set_r(self.gravity_mag * float(self.impulse_duration) / World.MICROS_IN_SEC)
gravity_acc = Vector3dm(0, 0, 0.0098, "c")
World.Log_Data("gravity_acc:{}->{}".format(
gravity_acc.convert_to_spherical(),
gravity_acc.convert_to_cartesian()))
return gravity_acc
def impulse(self):
for i in range(0, len(self.particles)):
el = self.particles[i]
el.acceleration = Vector3dm.zero_vector()
World.Log_Data("{}: impulse old: {}".format(i, el))
brain_acc = el.brain()
brain_acc.set_r(brain_acc.get_r() * float(self.impulse_duration) /
World.MICROS_IN_SEC)
World.Log_Data("{}: brain: {}".format(
i, brain_acc.convert_to_cartesian()))
aero_acc = el.aero_acc()
World.Log_Data("{}: pre set aero_acc: {}".format(
i, aero_acc.convert_to_cartesian()))
aero_acc.set_r(brain_acc.get_r() * float(self.impulse_duration) /
World.MICROS_IN_SEC)
World.Log_Data("{}: post set aero_acc: {}".format(
i, aero_acc.convert_to_cartesian()))
gravity_acc = self.gravity(el)
physics_acc = self.physics(el)
World.Log_Data("{}: impulse pre:{}".format(i, el))
el.acceleration = Vector3dm.zero_vector().add(brain_acc).add(
aero_acc).add(gravity_acc).add(physics_acc)
el.velocity = el.velocity.add(el.acceleration)
el.position = el.position.add(el.velocity)
World.Log_Data("{}: impulse post:{}".format(i, el))
self.constraints(
el) # change the particle based on wall/space constraints
World.Log_Data("{}: impulse new: {}".format(i, el))
self.collisions()
self.sim_time += self.impulse_duration
World.Log_Data("time: {}".format(self.sim_time))
def test_point_at_that(self):
#example from http://mathonline.wikidot.com/determining-a-vector-given-two-points
x1 = 2
y1 = 2
z1 = 1
x2 = 6
y2 = 3
z2 = 2
exp_x = 4
exp_y = 1
exp_z = 1
v1 = Vector3dm(x1,y1,z1,"c")
v2 = Vector3dm(x2,y2,z2,"c")
res_v = v1.point_at_that(v2).convert_to_cartesian()
rx = res_v.vals[0]
ry = res_v.vals[1]
rz = res_v.vals[2]
self.assertAlmostEqual(rx,exp_x,6,"point_at_that x bad result: is {} should be {}".format(rx,exp_x))
self.assertAlmostEqual(ry,exp_y,6,"point_at_that y bad result: is {} should be {}".format(ry,exp_y))
self.assertAlmostEqual(rz,exp_z,6,"point_at_that z bad result: is {} should be {}".format(rz,exp_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
def test_where_from_here(self):
#example from http://mathonline.wikidot.com/determining-a-vector-given-two-points; rearranged
x1 = 2
y1 = 2
z1 = 1
x2 = 4
y2 = 1
z2 = 1
exp_x = 6
exp_y = 3
exp_z = 2
v1 = Vector3dm(x1,y1,z1,"c")
v2 = Vector3dm(x2,y2,z2,"c")
res_v = v1.where_from_here(v2).convert_to_cartesian()
rx = res_v.vals[0]
ry = res_v.vals[1]
rz = res_v.vals[2]
self.assertAlmostEqual(rx,exp_x,6,"where_from_here x bad result: is {} should be {}".format(rx,exp_x))
self.assertAlmostEqual(ry,exp_y,6,"where_from_here y bad result: is {} should be {}".format(ry,exp_y))
self.assertAlmostEqual(rz,exp_z,6,"where_from_here z bad result: is {} should be {}".format(rz,exp_z))
self.assertEqual(type(v1.vals),type([]),"test changed vals type")
self.assertEqual(type(v2.vals),type([]),"test changed vals type")
self.assertEqual(type(res_v.vals),type([]),"test changed vals type")
