def ERM_step(ent):
'''
Euler-Richardson Method step
"Numerical integration of Newton's equations including velocity
dependent forces".
Ian R. Gatland. Am. J. Phys., Vol. 62, No. 3, March 1994
'''
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING*ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING*ent.angspeed
# Update vel(t+1/2) and position pos(t+1/2)
v_2 = ent.velocity + force*dt_2/ent.mass
w_2 = ent.angspeed + torque*dt_2/ent.inertia_moment
# Commented out. Used when forces depend on position and angle explicitly. Not programmed yet.
# p_2 = ent.position + v_2*dt_2
# a_2 = ent.ang + w_2*dt_2
# Update forces
# ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING*v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING*w_2
# Update vel(t+1)
ent.velocity = ent.velocity + force*dt_sec/ent.mass
ent.angspeed = ent.angspeed + torque*dt_sec/ent.inertia_moment
ent.position = ent.position + v_2*dt_sec
def ERM_step(ent):
'''
Euler-Richardson Method step
"Numerical integration of Newton's equations including velocity
dependent forces".
Ian R. Gatland. Am. J. Phys., Vol. 62, No. 3, March 1994
'''
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING * ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING * ent.angspeed
# Update vel(t+1/2) and position pos(t+1/2)
v_2 = ent.velocity + force * dt_2 / ent.mass
w_2 = ent.angspeed + torque * dt_2 / ent.inertia_moment
# Commented out. Used when forces depend on position and angle explicitly. Not programmed yet.
# p_2 = ent.position + v_2*dt_2
# a_2 = ent.ang + w_2*dt_2
# Update forces
# ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING * v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING * w_2
# Update vel(t+1)
ent.velocity = ent.velocity + force * dt_sec / ent.mass
ent.angspeed = ent.angspeed + torque * dt_sec / ent.inertia_moment
ent.position = ent.position + v_2 * dt_sec
ent.ang = ent.ang + w_2 * dt_sec
def verletV_step(ent):
'''
Perform one step of the verlet velocity algorithm
Not vectorized
'''
# Put the forces given in the entity frame into the global frame
# TODO: Put this in a function. Soon the entties wont be points
# anymore and more projections/rotations will be needed.
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING*ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING*ent.angspeed
# Update vel(t+1/2) and position pos(t+1)
v_2 = ent.velocity + force*dt_2/ent.mass
ent.position = ent.position + v_2*dt_sec
w_2 = ent.angspeed + torque*dt_2/ent.inertia_moment
ent.ang = ent.ang + w_2*dt_sec
# Update forces
#ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING*v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING*w_2
# Update vel(t+1)
ent.velocity = v_2 + force*dt_2/ent.mass
def verletV_step(ent):
'''
Perform one step of the verlet velocity algorithm
Not vectorized
'''
# Put the forces given in the entity frame into the global frame
# TODO: Put this in a function. Soon the entties wont be points
# anymore and more projections/rotations will be needed.
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING * ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING * ent.angspeed
# Update vel(t+1/2) and position pos(t+1)
v_2 = ent.velocity + force * dt_2 / ent.mass
ent.position = ent.position + v_2 * dt_sec
w_2 = ent.angspeed + torque * dt_2 / ent.inertia_moment
ent.ang = ent.ang + w_2 * dt_sec
# Update forces
#ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING * v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING * w_2
# Update vel(t+1)
ent.velocity = v_2 + force * dt_2 / ent.mass
ent.angspeed = w_2 + torque * dt_2 / ent.inertia_moment
