def set_angle_face_force(angle_vertex: Vertex, alfa0, face: Face, k_face):
# TODO: For potential speedup - calculations for the whole face at one time
alfa = face.angle_for_vertex(angle_vertex)
p1 = face.prev_vertex(angle_vertex)
p2 = angle_vertex
p3 = face.next_vertex(angle_vertex)
c = k_face * (alfa0 - alfa)
p21 = vector_from_to(p2.pos, p1.pos)
p21_len = p21.length
p23 = vector_from_to(p2.pos, p3.pos)
p23_len = p23.length
normal = face.normal
n_x_p21 = cross(normal, p21)
n_x_p23 = cross(normal, p23)
dp1 = n_x_p21 / p21_len
dp2 = -n_x_p21 / p21_len + n_x_p23 / p23_len
dp3 = -n_x_p23 / p23_len
f1 = c * dp1
f2 = c * dp2
f3 = c * dp3
p1.set_force(ForceName.FACE, Vector3.from_vec(f1))
p2.set_force(ForceName.FACE, Vector3.from_vec(f2))
p3.set_force(ForceName.FACE, Vector3.from_vec(f3))
def solve(self, output):
self._reset_forces()
self._reset_velocities()
self._set_forces()
self._set_target_angles()
cur_forces = self._total_forces_vecs()
if CONFIG['DEBUG']:
print('Starting solver')
print('FORCES: ', cur_forces)
plot_idx = 0
finished = False
while not finished:
if CONFIG['DEBUG']:
print(cur_forces)
for i, (v, total_force) in enumerate(zip(self.vertices,
cur_forces)):
node_mass = v.mass
v_t = v.velocity
p_t = v.pos
a = Vector3.from_vec(total_force) / node_mass
v_next = v_t + a * self.d_t
v.pos = p_t + v_next * self.d_t
v.velocity = v_next
if CONFIG['DEBUG']:
print(v)
print('VELOCITY: ', v.velocity)
print('TOTAL FORCE: ', v.total_force())
print('a = ', a)
print()
if CONFIG['DEBUG_PLOT']:
if plot_idx >= CONFIG['DEBUG_PLOT_FROM'] and plot_idx % CONFIG[
'DEBUG_PLOT_EVERY'] == 0:
from origuide.tools import plot
plot.plot3d(self.vertices, self.edges, self.faces,
cur_forces)
plot_idx += 1
if CONFIG['DEBUG']:
print('---')
self._reset_forces()
self._set_forces()
prev_forces = cur_forces.copy()
cur_forces = self._total_forces_vecs()
finished = self._should_end(prev_forces, cur_forces)
output.accept(self.vertices, finished)
return
