# Is it a good idea to solve movement of solid bodies with restrictions (e.g. connections) by shifting tangentially and projecting normally in phase sp?

I am trying to code my first 2d game with physics (in python), and so far my physics simulation works with solid bodies. I want to add connections, for example, so the player can rotate an arm and throw a ball. This appears nontrivial.

The algorithm for progressing the system forward by dt I currently have in head is as follows: (we work in an N-dimensional Euclidean space of coordinates and rotations of some material points, which are mass-normalized, so velocity has the same norm whenever kinetic energy is the same.

We start with position x, velocity v and acceleration a (the last of which can be arbitrarily influenced by external factors))

• Take the rows of the Jacobian matrix as the basis of the normal space

• Find the projection a_tan of the acceleration a to the tangent space (I guess this means solving a linear system with the quadratic form matrix of the Jacobian rows)

• approximate the next position x_1 as if it was moving in the entire phase space with velocity v and acceleration a_tan

• Move this x_1 along the normal vectors, using Newton's algorithm, until the values of all the restriction functions become smaller than some desired value MAX_DEVIATION, getting the point x_2

• Find the direction of the new velocity v_1 by requiring that J(v_1 - v) == 0 (the geodesic condition

• Get the other new velocity as v_2 = v_1 + a_tan * dt

• Calculate the Jacobian at x_2 (we can remember it at this point for the next step)

• Project v_2 to the zeros of the Jacobian to get v_3 (again, we can remember the inverse matrix of the quadratic form of the rows of the Jacobian for later)

• Set (x_2,v_3,something) as the new triplet (the "something" comes from outside forces)

Is this a good approach for this task? I did not study mechanics very deeply, so perhaps there is a completely different point of view on all this?

Or maybe there is just a python module I can use for this? A universal one --- I think this problem is normally solved in physics modules, but it works within their physical worlds with their collisions and forces, and I want to write those on my own.

• Is this a game or simulation? Black box and simplify. Apply hard constraints(bones) first, with inertia, limit as needed. Then move secondary and backpropogate the apposing forces with dampening. Diminish the system energy and repeat until total energy is close to 0. Commented Aug 24, 2023 at 23:39

I think what you are saying is probably a good idea. I have tried it and it works. Out of curiosity I wrote some python simulation for some example I cooked up. I can share it here, if it is of any interest.

I did not even need to find the tangential component of the acceleration to the constraint because I calculate the constraint accelerations take care of that. In a nutshell, the algorithm is:

Assume the acceleration of the system, without imposed constraints, is given by

a = acceleration(time, x, v)


and the constraints, written as a vector valued function/map, are

C(x) = L


Then, the equations of motion are

x'' = acceleration(t, x, v) + constraint_acceleration(x)
C(x) = L


To solve this system numerically, iterate the following sequence of calculations until time reaches prescribed stopping time:

1) given the current time, x, v, u

2) Set a = acceleration(time, x, v)

3) Solve for x_next and u_next the non-linear system:
x_next = x + dt*v + dt*dt*a + dt*[DC(x)^T]u_next
C(x_next) - L = 0
where DC(x)^T is the Jacobian of C(x) transposed

E.g. apply Newton's method with adjustment "bias" and initial guess u_next=u by iterating:
x_next = x + dt*v + dt*dt*a + dt*[DC(x)^T]u_next
u_next = u_next - bias*([DC(x)^T][DC(x)])^(-1) (C(x_next) - L)
until norm(C(x)-L) < small enough error

4) Set
time = time + dt
x = x_next
v = v + dt*a + [DC(x)^T]u_next
u = u_new

If time has not exceeded the prescribed sopping time, go back to 1)


Here is some python code, simulating an example, where there is standard gravitational acceleration, some air resistance and some time-dependent wind, that dies down to zero:

import math
import numpy as np
import matplotlib.pyplot as plt
from vpython import *

g=9.8
g_accel = -g * np.array([0,1,0,1,0,1,0,1])
e = np.array([1,0,1,0,1,0,1,0])

# A function that initializes the configuration of the system
def initialize(q):
L_1 = q[0:2].dot(q[0:2])
L_2 = q[0:2]-q[2:4]
L_2 = L_2.dot(L_2)
L_3 = q[2:4]-q[4:6]
L_3 = L_3.dot(L_3)
L_4 = q[0:2]-q[6:8]
L_4 = L_4.dot(L_4)
L_5 = q[4:6]-q[6:8]
L_5 = L_5.dot(L_5)
return np.array([L_1, L_2, L_3, L_4, L_5])

# A function that initializes the configuration of the system
def init_pos():
angle = np.pi * np.array([75, 60, -5, 10]) / 180
L = np.array([1.2, 1., 1.5, 1.3])
q = np.empty(8, dtype=float)
for i in range(4):
q[2*i : 2*i+2] = L[i]*np.array([np.sin(angle[i]), -np.cos(angle[i])])
for i in range(1,3):
q[2*i : 2*i+2] = q[2*i : 2*i+2] + q[2*i-2 : 2*i]
q[6 : 8] = q[6 : 8] + q[0 : 2]
return q

# Ture acceleration: in this example:
# constant gravity + wind that dies down with time + air resistance:
def acceleration(time, q, v):
a = g_accel + 7*e/(1 + ((time-1)/8)**8) - 0.01*math.sqrt(v.dot(v))*v
return a

# the constraints of the system, written as a map C(q) - L = 0
def C(q, L):
c_1 = q[0:2].dot(q[0:2]) - L[0]
c_2 = q[0:2]-q[2:4]
c_2 = c_2.dot(c_2) - L[1]
c_3 = q[2:4]-q[4:6]
c_3 = c_3.dot(c_3) - L[2]
c_4 = q[0:2]-q[6:8]
c_4 = c_4.dot(c_4) - L[3]
c_5 = q[4:6]-q[6:8]
c_5 = c_5.dot(c_5) - L[4]
return np.array([c_1, c_2, c_3, c_4, c_5])

# The Jacobian DC(q) of the constraint map, transposed
def DC_T(q):
q_12 = q[0:2] - q[2:4]
q_23 = q[2:4] - q[4:6]
q_14 = q[0:2] - q[6:8]
q_34 = q[4:6] - q[6:8]
J = np.array([[    q[0],    q[1],              0,0,              0,0,              0,0],
[ q_12[0], q_12[1],-q_12[0],-q_12[1],              0,0,              0,0],
[              0,0, q_23[0], q_23[1],-q_23[0],-q_23[1],              0,0],
[ q_14[0], q_14[1],              0,0,              0,0,-q_14[0],-q_14[1]],
[              0,0,              0,0, q_34[0], q_34[1],-q_34[0],-q_34[1]]])
return J.T

# the propagation of the system for a single time-step
def one_step(time, x, v, u_old, L, bias, dt):
a = acceleration(time, x, v)
v_half = v + dt * a
x_half = x + dt * v_half
Dc_T = DC_T(x)
M = Dc_T.T.dot(Dc_T)
M = np.linalg.inv(M)
u = u_old
for i in range(8):
dv_out = Dc_T.dot(u)
x_out = x_half + dt*dv_out
c = C(x_out, L)
error = np.abs(c).max()
if error < 1e-7: break
u = u - bias * M.dot(c)
return time + dt, x_out, v_half + dv_out, u, error

# the full propagation of the dynamics
def propagate(x_start, v_start, L, n_time_steps, dt):
u = np.zeros(L.shape[0], dtype=float)
bias = 20
x, v = x_start, v_start
X = np.empty((n_time_steps+1, x.shape[0]), dtype=float)
Error = np.empty((n_time_steps+1,), dtype=float)
X[0] = x
time = 0
for t in range(1, n_time_steps+1):
time, x, v, u, error = one_step(time, x, v, u, L, bias, dt)
X[t] = x
Error[t] = error
return X, Error

# The animation of the actual simulation:
# starting with the initialization of the system
q = init_pos()
L =  initialize(q)
w = np.zeros(q.shape[0], dtype=float)
u = np.zeros(L.shape[0], dtype=float)
dt = 0.03
n = 1000
t_stop = n*dt

# generation of the dynamics for the system.
# X is an array that contains the configuration
# for the system for every time-step.
# Err is the array of all errors, i.e. how much the
# configuration fails to satisfy the constraints.
X, Err = propagate(q, w, L, n, dt)

# print the min and max errors (optional):
print(Err.min(), Err.max())

# animation using Vpython:
width=1700, height=900,
center=vector(0,0,0), background=color.black)

shift = vector(0, np.sum(L[0:3])/2, 0)

fixed=sphere(pos = shift,
color = color.blue,
make_trail=False)

bob = []
bar = []

for i in range(q.shape[0] // 2):
bob.append( sphere(pos = shift + vector(q[2*i], q[2*i+1], 0),
color = color.red,
make_trail=False))

for i in range(L.shape[0]-1):
bar.append( cylinder(pos = bob[i].pos,
axis = vector(0,0,0),
color = color.red))

bar.append( cylinder(pos = bob[3].pos,
axis = vector(0,0,0),
color = color.red))

bar[0].axis = fixed.pos - bar[0].pos
bar[1].axis = bar[0].pos - bar[1].pos
bar[2].axis = bar[1].pos - bar[2].pos
bar[3].axis = bar[0].pos - bar[3].pos
bar[4].axis = bar[2].pos - bar[3].pos

#u = np.zeros(L.shape[0], dtype=float)
t = 0
for t in range(X.shape[0]):
rate(30)
for i in range(q.shape[0] // 2):
bob[i].pos = shift + vector(X[t, 2*i], X[t, 2*i+1], 0)
bar[i].pos = bob[i].pos
bar[4].pos = bob[3].pos
bar[0].axis = fixed.pos - bar[0].pos
bar[1].axis = bar[0].pos - bar[1].pos
bar[2].axis = bar[1].pos - bar[2].pos
bar[3].axis = bar[0].pos - bar[3].pos
bar[4].axis = bar[2].pos - bar[3].pos