def step(self,times):
for i in range(0,times):
if not self.on_ground:
w=self.get_corr_w(self.o_tens,self.ang_mom)
w_hat=vf.unit(w)
rad=vf.mag(w)*self.dt
step_com_node=self.get_translate(self.com_node,self.dt)
step_corners=self.get_rotate_corners(self.centered_corners,w_hat,rad)
self.collision_check(step_com_node,step_corners,w_hat,rad)
else:
w=self.get_corr_w(self.o_tens,self.ang_mom)
rad=vf.mag(w)*self.dt
if rad!=0:
w_hat=vf.unit(w)
self.centered_corners=self.get_rotate_corners(self.centered_corners,w_hat,rad)
self.o_tens=self.get_rot_o_tens2(self.o_tens,w_hat,rad)
self.ang_mom=self.ang_mom-self.fric*self.ang_mom*self.dt
self.t+=self.dt
def do_collide_after(self,step_com_node,step_corners,corners,step_o_tens,o_tens,ang_mom,com_node):
#does collision at step in which corner is below floor, but applies those values to block at step before collision
nodes_below=self.get_nodes_below2(step_com_node,step_corners)
r=ar([0,0,0])
count=0
for n in nodes_below:
r=r+n.p
count+=1
r=r/count
(delt_v,delt_L)=self.get_delt_v_L(r=r,o_tens=step_o_tens,ang_mom=ang_mom,com_node=step_com_node)
com_node.v=com_node.v+delt_v
ang_mom=ang_mom+delt_L
w=self.get_corr_w(o_tens,ang_mom)
w_hat=vf.unit(w)
rad=vf.mag(w)*self.dt_col
step_com_node=self.get_translate(com_node,self.dt_col)
step_corners=self.get_rotate_corners(corners,w_hat,rad)
return step_com_node,step_corners,ang_mom,com_node,w_hat,rad
def do_collide_before(self,step_com_node,step_corners,corners,o_tens,ang_mom,com_node):
#does collision as if happening at step before corner goes below floor
nodes_below=self.get_nodes_below(step_com_node,step_corners,corners)
r=ar([0,0,0])
count=0
for n in nodes_below:
r=r+n.p
count+=1
r=r/count
(delt_v,delt_L)=self.get_delt_v_L(r=r,o_tens=o_tens,ang_mom=ang_mom,com_node=com_node)
com_node.v=com_node.v+delt_v
ang_mom=ang_mom+delt_L
w=self.get_corr_w(o_tens,ang_mom)
w_hat=vf.unit(w)
rad=vf.mag(w)*self.dt_col
step_com_node=self.get_translate(com_node,self.dt_col)
step_corners=self.get_rotate_corners(corners,w_hat,rad)
return step_com_node,step_corners,ang_mom,com_node,w_hat,rad
def collide(self):
self.dt_col=self.dt/self.col_N
com_node=self.com_node
corners=self.centered_corners
o_tens=self.o_tens
ang_mom=self.ang_mom
print('collide initiated')
for i in range(0,self.col_N):
w=self.get_corr_w(o_tens,ang_mom)
w_hat=vf.unit(w)
rad=vf.mag(w)*self.dt_col
step_com_node=self.get_translate(com_node,self.dt_col)
step_corners=self.get_rotate_corners(corners,w_hat,rad)
if not self.on_ground:
if not self.nodes_below(step_com_node,step_corners):
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
else:
(step_com_node,step_corners,ang_mom,com_node,w_hat,rad)=self.do_collide_before(step_com_node,step_corners,corners,o_tens,ang_mom,com_node)
if not self.nodes_below(step_com_node,step_corners):
print('all good')
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
else:
if self.get_trans_KE(com_node)+self.get_alt_rot_KE(o_tens,ang_mom)<=self.ground_E_tol:
print('on ground, energy less than ground tol')
com_node.v=ar([0,0,0])
com_node.a=ar([0,0,0])
ang_mom=ar([0,0,ang_mom[2]])
self.on_ground=True
else:
step_o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
(step_com_node,step_corners,ang_mom,com_node,w_hat,rad)=self.do_collide_after(step_com_node,step_corners,corners,step_o_tens,o_tens,ang_mom,com_node)
if not self.nodes_below(step_com_node,step_corners):
print('all good 2')
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(step_o_tens,w_hat,rad)
else:
step_o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
(step_com_node,step_corners,ang_mom,com_node,w_hat,rad)=self.do_collide_after(step_com_node,step_corners,corners,step_o_tens,o_tens,ang_mom,com_node)
if not self.nodes_below(step_com_node,step_corners):
print('all good 3')
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(step_o_tens,w_hat,rad)
else:
step_o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
(step_com_node,step_corners,ang_mom,com_node,w_hat,rad)=self.do_collide_after(step_com_node,step_corners,corners,step_o_tens,o_tens,ang_mom,com_node)
if not self.nodes_below(step_com_node,step_corners):
print('all good 4')
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(step_o_tens,w_hat,rad)
else:
step_o_tens=self.get_rot_o_tens2(o_tens,w_hat,rad)
(step_com_node,step_corners,ang_mom,com_node,w_hat,rad)=self.do_collide_after(step_com_node,step_corners,corners,step_o_tens,o_tens,ang_mom,com_node)
if not self.nodes_below(step_com_node,step_corners):
print('all good 5')
com_node=step_com_node
corners=step_corners
o_tens=self.get_rot_o_tens2(step_o_tens,w_hat,rad)
else:
print('fail, on ground')
com_node.v=ar([0,0,0])
com_node.a=ar([0,0,0])
ang_mom=ar([0,0,ang_mom[2]])
self.on_ground=True
else:
w=self.get_corr_w(self.o_tens,self.ang_mom)
rad=vf.mag(w)*self.dt
if rad!=0:
w_hat=vf.unit(w)
corners=self.get_rotate_corners(self.centered_corners,w_hat,rad)
o_tens=self.get_rot_o_tens2(self.o_tens,w_hat,rad)
ang_mom=self.ang_mom-self.fric*self.ang_mom*self.dt
else:
print('done spinning')
break
print('collide complete')
return com_node,o_tens,corners,ang_mom
def get_rot_o_tens2(self,o_tens,w_hat,rad):
return ar([vf.unit(rotate.point(point=ax,w_hat=w_hat,rad=rad)) for ax in o_tens])
def get_rot_o_tens(self,o_tens,w):
return ar([vf.unit(rotate.point(point=ax,w_hat=vf.unit(w),rad=vf.mag(w*self.dt))) for ax in o_tens])