The torque of a motor is proportional to the current flowing through it ().

The angular velocity of a motor is proportional to the voltage across it ().

Details and derivation here: motor.pdf.

• All variables are represented in italics except in the diagrams and Python code.
• Vectors are shown in bold except in the diagrams and Python code.
• The notation |a| is used to represent the length of a vector named a.

Introduction to the problem

Start by defining a few points (vectors from the world origin):

Now, from these points, you can easily calculate two useful values, the segment vector, seg_v (from seg_a to seg_b) and the position of circ_pos relative to seg_a, pt_v.

seg_v = seg_b – seg_a

pt_v = circ_pos – seg_a

Closest point to the circle’s center on the segment

The next step is to find the closest point to the circle’s center on the segment (labeled “closest” on the diagram). To do this, we must project pt_v onto seg_v:

To project on vector onto another, take the dot product of the vector and the unit vector of the projection target. That is:

Note that this dot product returns a scalar value (the length of proj_v), not a vector.

–

We now need to take a small break from the main goal and look into a special case: the ends of the segment. If |proj_v| is less than 0 or greater than |seg_v|, the closest point to circ_pos on the segment will be one of the segment’s endpoints. That is:
if |proj_v| < 0: closest = seg_a
if |proj_v| > |seg_v|: closest = seg_b

–

Next, calculate the actual proj_v vector, rather than just its length (|proj_v|). To do this, simply multiply by the seg_v unit vector:

The only thing left to do is to convert this into world coordinates (rather than coordinates relative to seg_a) to get the closest point:

closest = seg_a + proj_v

Checking for a intersection

Now that we’ve calculated the closest point on the segment (the variable “closest“), we can check if the circle and segment intersect. To do this, first find the vector from closest to circ_pos:

dist_v = circ_pos – closest

If the length of this vector is less than the circle’s radius, the circle and segment are intersecting:
if |dist_v| < circ_rad: They are intersecting
else: They are not intersecting

Collision response

A useful bit of data in collision response is the amount of overlap between two shapes and the direction a shape must be moved in order to resolve the collision. This can be represented as a vector that points in the same direction as dist_v whose length is equal to the difference between circ_rad and |dist_v|.

Python implementation

Here is a Python implementation of everything in this post. For the sake of clarity, I avoided optimizations. vec is assumed to be a fully implemented 2d vector class.

def closest_point_on_seg(seg_a, seg_b, circ_pos):
	seg_v = seg_b - seg_a
	pt_v = circ_pos - seg_a
	if seg_v.len() <= 0:
		raise ValueError, "Invalid segment length"
	seg_v_unit = seg_v / seg_v.len()
	proj = pt_v.dot(seg_v_unit)
	if proj <= 0:
		return seg_a.copy()
	if proj >= seg_v.len():
		return seg_b.copy()
	proj_v = seg_v_unit * proj
	closest = proj_v + seg_a
	return closest
 
def segment_circle(seg_a, seg_b, circ_pos, circ_rad):
	closest = closest_point_on_seg(seg_a, seg_b, circ_pos)
	dist_v = circ_pos - closest
	if dist_v.len() > circ_rad:
		return vec(0, 0)
	if dist_v.len() <= 0:
		raise ValueError, "Circle's center is exactly on segment"
	offset = dist_v / dist_v.len() * (circ_rad - dist_v.len())
	return offset

Similar algorithms

Collisions involving stadiums (a type of rounded rectangle) can be calculated in a similar manner. A stadium is essentially a line segment with a radius.

A stadium-point collision is the same as a segment-circle collision with a circle whose radius is equal to the stadium’s radius.

A stadium-circle collision is the same as a segment-circle collision with a circle whose radius is equal to the sum of the original circle’s radius and the stadium’s radius.

Download

segment_circle.zip – An implementation in Python and Cython.

Conclusion

If you find this post useful or have any questions, please leave a comment.

This is a Python implementation of the RK4 numerical integrator that works with differential functions of all orders. That is, any function in the form F(x, y, y’, y”, …, ).

If you’re new to numerical integration or even RK4 integration, please read my other post first. It’s easier to understand because it’s a less generalized function.

def rk4(t0, h, s0, f):
	"""RK4 implementation.
	t = current value of the independent variable
	h = amount to increase the independent variable (step size)
	s0 = initial state as a list. ex.: [initial_position, initial_velocity]
	f = function(state, t) to integrate"""
	r = range(len(s0))
	s1 = s0 + [f(t0, s0)]
	s2 = [s0[i] + 0.5*s1[i+1]*h for i in r]
	s2 += [f(t0+0.5*h, s2)]
	s3 = [s0[i] + 0.5*s2[i+1]*h for i in r]
	s3 += [f(t0+0.5*h, s3)]
	s4 = [s0[i] + s3[i+1]*h for i in r]
	s4 += [f(t0+h, s4)]
	return t+h, [s0[i] + (s1[i+1] + 2*(s2[i+1]+s3[i+1]) + s4[i+1])*h/6.0 for i in r]

An example usage of this function is:

def damped_spring_accel(t, state):
	x = state[0]
	v = state[1]
	stiffness = 1
	damping = 0.005
	return -stiffness*x - damping*v
 
t = 0
h = 1/40.0
s = [50, 5]
 
while t<100:
	t, s = rk4(t, h, s, damped_spring_accel)
print "Final state: t=%.2f x=%.2f v=%.2f"%(t,s[0],s[1])

On a side note, I recently found out about Sage (sagemath.org and sagenb.org). Its plotting capabilities and convenient mathematical notation are especially useful. Plus, it uses Python!

(View the full post to try it out.)

Source code: Chain Link

To try out the demo, go to http://doswa.com/projects/physics_js/

Chain of particles attached by joints

def rk4(x, v, a, dt):
	"""Returns final (position, velocity) tuple after
	time dt has passed.
 
	x: initial position (number-like object)
	v: initial velocity (number-like object)
	a: acceleration function a(x,v,dt) (must be callable)
	dt: timestep (number)"""
	x1 = x
	v1 = v
	a1 = a(x1, v1, 0)
 
	x2 = x + 0.5*v1*dt
	v2 = v + 0.5*a1*dt
	a2 = a(x2, v2, dt/2.0)
 
	x3 = x + 0.5*v2*dt
	v3 = v + 0.5*a2*dt
	a3 = a(x3, v3, dt/2.0)
 
	x4 = x + v3*dt
	v4 = v + a3*dt
	a4 = a(x4, v4, dt)
 
	xf = x + (dt/6.0)*(v1 + 2*v2 + 2*v3 + v4)
	vf = v + (dt/6.0)*(a1 + 2*a2 + 2*a3 + a4)
 
	return xf, vf

Here is an example usage of the function and a comparison to Euler integration:

def accel(x, v, dt):
	"""Determines acceleration from current position,
	velocity, and timestep. This particular acceleration
	function models a spring."""
	stiffness = 1
	damping = -0.005
	return -stiffness*x - damping*v
 
t = 0
dt = 1.0/40 # Timestep of 1/40 second
state = 50, 5 # Position, velocity
euler = 50, 5 # For comparison with Euler integration
 
print "Initial    -position: %6.2f, velocity: %6.2f"%state
 
# Run for 100 seconds
while t < 100:
	t += dt
	state = rk4(state[0], state[1], accel, dt)
 
	# Integrate using Euler's method
	euler = (
		euler[0] + euler[1]*dt,
		euler[1] + accel(euler[0],euler[1],dt)*dt
	)
 
print "Final RK4  -position: %6.2f, velocity: %6.2f"%state
print "Final Euler-position: %6.2f, velocity: %6.2f"%euler

The output of this really shows how much more accurate RK4 integration can be:

Initial    -position:  50.00, velocity:   5.00
Final RK4  -position:  52.18, velocity:  38.05
Final Euler-position: 178.38, velocity: 137.62

As the timestep is decreased (meaning more computation), Euler approaches RK4 (shown at timestep of 1/400 seconds):

Initial    -position:  50.00, velocity:   5.00
Final RK4  -position:  52.28, velocity:  37.92
Final Euler-position:  59.20, velocity:  43.02