├── .gitignore ├── 0_getting_started ├── readme.md ├── simulator0.py ├── simulator1.py ├── simulator2.py └── square_mesh.py ├── 10_mpm_elasticity ├── readme.md ├── results.gif └── simulator.py ├── 11_mpm_sand ├── readme.md ├── results.gif └── simulator.py ├── 1_mass_spring ├── InertiaEnergy.py ├── MassSpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 2_dirichlet ├── GravityEnergy.py ├── InertiaEnergy.py ├── MassSpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 3_contact ├── BarrierEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── MassSpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 4_friction ├── BarrierEnergy.py ├── FrictionEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── MassSpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 5_mov_dirichlet ├── BarrierEnergy.py ├── FrictionEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── MassSpringEnergy.py ├── SpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 6_inv_free ├── BarrierEnergy.py ├── FrictionEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── NeoHookeanEnergy.py ├── SpringEnergy.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 7_self_contact ├── BarrierEnergy.py ├── FrictionEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── NeoHookeanEnergy.py ├── SpringEnergy.py ├── distance │ ├── CCD.py │ ├── PointEdgeDistance.py │ ├── PointLineDistance.py │ └── PointPointDistance.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 8_self_friction ├── BarrierEnergy.py ├── FrictionEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── NeoHookeanEnergy.py ├── SpringEnergy.py ├── distance │ ├── CCD.py │ ├── PointEdgeDistance.py │ ├── PointLineDistance.py │ └── PointPointDistance.py ├── readme.md ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── 9_reduced_DOF ├── BarrierEnergy.py ├── GravityEnergy.py ├── InertiaEnergy.py ├── NeoHookeanEnergy.py ├── readme.md ├── results.gif ├── simulator.py ├── square_mesh.py ├── time_integrator.py └── utils.py ├── LICENSE └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | test.py 3 | *.pyc 4 | output/ -------------------------------------------------------------------------------- /0_getting_started/readme.md: -------------------------------------------------------------------------------- 1 | # Getting Started with Solid Simulation 2 | 3 | Free fall, spring, and mass-spring solid simulation with symplectic Euler time integration. 4 | 5 | ## Dependencies 6 | ``` 7 | pip install numpy scipy pygame 8 | ``` 9 | 10 | ## Run 11 | Free falling particle: 12 | ``` 13 | python simulator0.py 14 | ``` 15 | Hanging particle: 16 | ``` 17 | python simulator1.py 18 | ``` 19 | Hanging square: 20 | ``` 21 | python simulator2.py 22 | ``` -------------------------------------------------------------------------------- /0_getting_started/simulator0.py: -------------------------------------------------------------------------------- 1 | # Free Fall Simulation 2 | 3 | import math 4 | import numpy as np # for vector data structure and computations 5 | import pygame # for visualization 6 | 7 | # simulation setup 8 | x = np.array([0.0, 0.0]) # position of particle 9 | v = np.array([0.0, 0.0]) # velocity of particle 10 | g = np.array([0.0, -10.0]) # gravitational acceleration 11 | h = 0.01 # time step size in seconds 12 | 13 | # visualization/rendering setup 14 | pygame.init() 15 | render_FPS = 100 # number of frames to render per second 16 | resolution = np.array([900, 900]) # visualization window size in pixels 17 | offset = resolution / 2 # offset between window coordinates and simulated coordinates 18 | scale = 200 # scale between window coordinates and simulated coordinates 19 | def screen_projection(x): # convert simulated coordinates to window coordinates 20 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 21 | screen = pygame.display.set_mode(resolution) # initialize visualizer 22 | 23 | time_step = 0 # the number of the current time step 24 | running = True # flag indicating whether the simulation is still running 25 | while running: 26 | # run until the user asks to quit 27 | for event in pygame.event.get(): 28 | if event.type == pygame.QUIT: 29 | running = False 30 | 31 | # update the frame to display according to render_FPS 32 | if time_step % int(math.ceil((1.0 / render_FPS) / h)) == 0: 33 | # fill the background with white color, display simulation time at the top, 34 | # render a floor at y=-1, and render the particle as a circle: 35 | screen.fill((255, 255, 255)) 36 | pygame.display.set_caption('Current time: ' + f'{time_step * h: .2f}s') 37 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([-2, -1]), screen_projection([2, -1])) 38 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(x), 0.1 * scale) 39 | pygame.display.flip() # flip the display 40 | pygame.time.wait(int(1000.0 / render_FPS)) # wait to render the next frame 41 | 42 | # pause the simulation when the particle touches on the ground 43 | if x[1] <= -1: 44 | input() 45 | break 46 | 47 | # step forward the simulation by updating particle velocity and position 48 | v += h * g 49 | x += h * v 50 | 51 | time_step += 1 # update time step counter -------------------------------------------------------------------------------- /0_getting_started/simulator1.py: -------------------------------------------------------------------------------- 1 | # Spring Simulation 2 | 3 | import math 4 | import numpy as np # for vector data structure and computations 5 | import pygame # for visualization 6 | 7 | # simulation setup 8 | m = 1000 # mass of particle 9 | x = np.array([0.4, 0.0]) # position of particle 10 | v = np.array([0.0, 0.0]) # velocity of particle 11 | g = np.array([0.0, -10.0]) # gravitational acceleration 12 | spring_rest_len = 0.3 # rest length of the spring ### 13 | spring_stiffness = 1e5 # stiffness of the spring ### 14 | h = 0.01 # time step size in seconds 15 | 16 | # visualization/rendering setup 17 | pygame.init() 18 | render_FPS = 100 # number of frames to render per second 19 | resolution = np.array([900, 900]) # visualization window size in pixels 20 | offset = resolution / 2 # offset between window coordinates and simulated coordinates 21 | scale = 200 # scale between window coordinates and simulated coordinates 22 | def screen_projection(x): # convert simulated coordinates to window coordinates 23 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 24 | screen = pygame.display.set_mode(resolution) # initialize visualizer 25 | 26 | time_step = 0 # the number of the current time step 27 | running = True # flag indicating whether the simulation is still running 28 | while running: 29 | # run until the user asks to quit 30 | for event in pygame.event.get(): 31 | if event.type == pygame.QUIT: 32 | running = False 33 | 34 | # update the frame to display according to render_FPS 35 | if time_step % int(math.ceil((1.0 / render_FPS) / h)) == 0: 36 | # fill the background with white color, display simulation time at the top, 37 | # draw the spring segment, and render the particle as a circle: 38 | screen.fill((255, 255, 255)) 39 | pygame.display.set_caption('Current time: ' + f'{time_step * h: .2f}s') 40 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([0, 0]), screen_projection(x)) ### 41 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(x), 0.1 * scale) 42 | pygame.display.flip() # flip the display 43 | pygame.time.wait(int(1000.0 / render_FPS)) # wait to render the next frame 44 | 45 | # step forward the simulation by updating particle velocity and position 46 | spring_cur_len = math.sqrt(x[0] * x[0] + x[1] * x[1]) ### 47 | spring_displacement = spring_cur_len - spring_rest_len ### 48 | spring_force = -spring_stiffness * spring_displacement * (x / spring_cur_len) ### 49 | v += h * (g + spring_force / m) 50 | x += h * v 51 | 52 | time_step += 1 # update time step counter -------------------------------------------------------------------------------- /0_getting_started/simulator2.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solid Simulation 2 | 3 | import math 4 | import numpy as np # for vector data structure and computations 5 | import pygame # for visualization 6 | import square_mesh # for generating a square mesh 7 | 8 | # simulation setup 9 | side_length = 1 # side length of the square 10 | n_seg = 4 # number of springs per side of the square 11 | m = 1000 # mass of each particle 12 | [x, e] = square_mesh.generate(side_length, n_seg) # array of particle positions and springs ### 13 | v = np.array([[0.0, 0.0]] * len(x)) # velocity array of particles ### 14 | g = np.array([0.0, -10.0]) # gravitational acceleration 15 | spring_rest_len = [] # rest length array of the springs ### 16 | for i in range(0, len(e)): # calculate the rest length of each spring 17 | spring_vec = x[e[i][0]] - x[e[i][1]] # the vector connecting two ends of spring i 18 | spring_rest_len.append(math.sqrt(spring_vec[0] * spring_vec[0] + spring_vec[1] * spring_vec[1])) 19 | spring_stiffness = 1e6 # stiffness of the spring 20 | h = 0.01 # time step size in seconds 21 | 22 | # visualization/rendering setup 23 | pygame.init() 24 | render_FPS = 100 # number of frames to render per second 25 | resolution = np.array([900, 900]) # visualization window size in pixels 26 | offset = resolution / 2 # offset between window coordinates and simulated coordinates 27 | scale = 200 # scale between window coordinates and simulated coordinates 28 | def screen_projection(x): # convert simulated coordinates to window coordinates 29 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 30 | screen = pygame.display.set_mode(resolution) # initialize visualizer 31 | 32 | time_step = 0 # the number of the current time step 33 | running = True # flag indicating whether the simulation is still running 34 | while running: 35 | # run until the user asks to quit 36 | for event in pygame.event.get(): 37 | if event.type == pygame.QUIT: 38 | running = False 39 | 40 | # update the frame to display according to render_FPS 41 | if time_step % int(math.ceil((1.0 / render_FPS) / h)) == 0: 42 | # fill the background with white color, display simulation time at the top, 43 | # draw each spring segment, and render each particle as a circle: 44 | screen.fill((255, 255, 255)) 45 | pygame.display.set_caption('Current time: ' + f'{time_step * h: .2f}s') 46 | for i in range(0, len(e)): ### 47 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[e[i][0]]), screen_projection(x[e[i][1]])) 48 | for i in range(0, len(x)): ### 49 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(x[i]), 0.02 * scale) 50 | pygame.display.flip() # flip the display 51 | pygame.time.wait(int(1000.0 / render_FPS)) # wait to render the next frame 52 | 53 | # step forward the simulation by updating particle velocity and position ### 54 | for i in range(0, len(e)): 55 | # calculate elasticity force of spring i: 56 | spring_vec = x[e[i][0]] - x[e[i][1]] 57 | spring_cur_len = math.sqrt(spring_vec[0] * spring_vec[0] + spring_vec[1] * spring_vec[1]) 58 | spring_displacement = spring_cur_len - spring_rest_len[i] 59 | spring_force = -spring_stiffness * spring_displacement * (spring_vec / spring_cur_len) 60 | # update the velocity of the two ends of spring i 61 | v[e[i][0]] += h * (g + spring_force / m) 62 | v[e[i][1]] += h * (g - spring_force / m) 63 | # fix the top left and top right corner by setting velocity to 0: 64 | v[n_seg] = v[(n_seg + 1) * (n_seg + 1) - 1] = np.array([0, 0]) 65 | # update the position of each particle: 66 | for i in range(0, len(x)): 67 | x[i] += h * v[i] 68 | 69 | time_step += 1 # update time step counter -------------------------------------------------------------------------------- /0_getting_started/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def generate(side_length, n_seg): 4 | # sample nodes uniformly on a square 5 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 6 | step = side_length / n_seg 7 | for i in range(0, n_seg + 1): 8 | for j in range(0, n_seg + 1): 9 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 10 | 11 | # connect the nodes with edges 12 | e = [] 13 | # horizontal edges 14 | for i in range(0, n_seg): 15 | for j in range(0, n_seg + 1): 16 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 17 | # vertical edges 18 | for i in range(0, n_seg + 1): 19 | for j in range(0, n_seg): 20 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 21 | # diagonals 22 | for i in range(0, n_seg): 23 | for j in range(0, n_seg): 24 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 25 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 26 | 27 | return [x, e] -------------------------------------------------------------------------------- /10_mpm_elasticity/readme.md: -------------------------------------------------------------------------------- 1 | # Two Colliding Elastic Blocks in 2D 2 | 3 | Two elastic blocks with opposite initial velocities collide head-on using the simplest PIC transfer scheme, illustrating a minimal yet complete implementation of the Material Point Method (MPM). 4 | 5 | ![results](results.gif) 6 | 7 | ## Dependencies 8 | ``` 9 | pip install numpy taichi 10 | ``` 11 | 12 | ## Run 13 | ``` 14 | python simulator.py 15 | ``` -------------------------------------------------------------------------------- /10_mpm_elasticity/results.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/phys-sim-book/solid-sim-tutorial/f9d4811b2f95bdf7c189669fb9b614b8baab6fcf/10_mpm_elasticity/results.gif -------------------------------------------------------------------------------- /10_mpm_elasticity/simulator.py: -------------------------------------------------------------------------------- 1 | # Material Point Method Simulation 2 | import numpy as np # numpy for linear algebra 3 | import taichi as ti # taichi for fast and parallelized computation 4 | 5 | ti.init(arch=ti.cpu) 6 | 7 | # ANCHOR: property_def 8 | # simulation setup 9 | grid_size = 128 # background Eulerian grid's resolution, in 2D is [128, 128] 10 | dx = 1.0 / grid_size # the domain size is [1m, 1m] in 2D, so dx for each cell is (1/128)m 11 | dt = 2e-4 # time step size in second 12 | ppc = 8 # average particles per cell 13 | 14 | density = 1000 # mass density, unit: kg / m^3 15 | E, nu = 1e4, 0.3 # block's Young's modulus and Poisson's ratio 16 | mu, lam = E / (2 * (1 + nu)), E * nu / ((1 + nu) * (1 - 2 * nu)) # Lame parameters 17 | # ANCHOR_END: property_def 18 | 19 | # ANCHOR: setting 20 | # uniformly sampling material particles 21 | def uniform_grid(x0, y0, x1, y1, dx): 22 | xx, yy = np.meshgrid(np.arange(x0, x1 + dx, dx), np.arange(y0, y1 + dx, dx)) 23 | return np.column_stack((xx.ravel(), yy.ravel())) 24 | 25 | box1_samples = uniform_grid(0.2, 0.4, 0.4, 0.6, dx / np.sqrt(ppc)) 26 | box1_velocities = np.tile(np.array([10.0, 0]), (len(box1_samples), 1)) 27 | box2_samples = uniform_grid(0.6, 0.4, 0.8, 0.6, dx / np.sqrt(ppc)) 28 | box2_velocities = np.tile(np.array([-10.0, 0]), (len(box1_samples), 1)) 29 | all_samples = np.concatenate([box1_samples, box2_samples], axis=0) 30 | all_velocities = np.concatenate([box1_velocities, box2_velocities], axis=0) 31 | # ANCHOR_END: setting 32 | 33 | # ANCHOR: data_def 34 | # material particles data 35 | N_particles = len(all_samples) 36 | x = ti.Vector.field(2, float, N_particles) # the position of particles 37 | x.from_numpy(all_samples) 38 | v = ti.Vector.field(2, float, N_particles) # the velocity of particles 39 | v.from_numpy(all_velocities) 40 | vol = ti.field(float, N_particles) # the volume of particle 41 | vol.fill(0.2 * 0.4 / N_particles) # get the volume of each particle as V_rest / N_particles 42 | m = ti.field(float, N_particles) # the mass of particle 43 | m.fill(vol[0] * density) 44 | F = ti.Matrix.field(2, 2, float, N_particles) # the deformation gradient of particles 45 | F.from_numpy(np.tile(np.eye(2), (N_particles, 1, 1))) 46 | 47 | # grid data 48 | grid_m = ti.field(float, (grid_size, grid_size)) 49 | grid_v = ti.Vector.field(2, float, (grid_size, grid_size)) 50 | # ANCHOR_END: data_def 51 | 52 | # ANCHOR: reset_grid 53 | def reset_grid(): 54 | # after each transfer, the grid is reset 55 | grid_m.fill(0) 56 | grid_v.fill(0) 57 | # ANCHOR_END: reset_grid 58 | 59 | ################################ 60 | # Stvk Hencky Elasticity 61 | ################################ 62 | # ANCHOR: stvk 63 | @ti.func 64 | def StVK_Hencky_PK1_2D(F): 65 | U, sig, V = ti.svd(F) 66 | inv_sig = sig.inverse() 67 | e = ti.Matrix([[ti.log(sig[0, 0]), 0], [0, ti.log(sig[1, 1])]]) 68 | return U @ (2 * mu * inv_sig @ e + lam * e.trace() * inv_sig) @ V.transpose() 69 | # ANCHOR_END: stvk 70 | 71 | # Particle-to-Grid (P2G) Transfers 72 | # ANCHOR: p2g 73 | @ti.kernel 74 | def particle_to_grid_transfer(): 75 | for p in range(N_particles): 76 | base = (x[p] / dx - 0.5).cast(int) 77 | fx = x[p] / dx - base.cast(float) 78 | # quadratic B-spline interpolating functions (Section 26.2) 79 | w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2] 80 | # gradient of the interpolating function (Section 26.2) 81 | dw_dx = [fx - 1.5, 2 * (1.0 - fx), fx - 0.5] 82 | 83 | P = StVK_Hencky_PK1_2D(F[p]) 84 | for i in ti.static(range(3)): 85 | for j in ti.static(range(3)): 86 | offset = ti.Vector([i, j]) 87 | weight = w[i][0] * w[j][1] 88 | grad_weight = ti.Vector([(1. / dx) * dw_dx[i][0] * w[j][1], 89 | w[i][0] * (1. / dx) * dw_dx[j][1]]) 90 | 91 | grid_m[base + offset] += weight * m[p] # mass transfer 92 | grid_v[base + offset] += weight * m[p] * v[p] # momentum Transfer, PIC formulation 93 | # internal force (stress) transfer 94 | fi = -vol[p] * P @ F[p].transpose() @ grad_weight 95 | grid_v[base + offset] += dt * fi 96 | # ANCHOR_END: p2g 97 | 98 | # Grid Update 99 | # ANCHOR: grid_update 100 | @ti.kernel 101 | def update_grid(): 102 | for i, j in grid_m: 103 | if grid_m[i, j] > 0: 104 | grid_v[i, j] = grid_v[i, j] / grid_m[i, j] # extract updated nodal velocity from transferred nodal momentum 105 | 106 | # Dirichlet BC near the bounding box 107 | if i <= 3 or i > grid_size - 3 or j <= 2 or j > grid_size - 3: 108 | grid_v[i, j] = 0 109 | # ANCHOR_END: grid_update 110 | 111 | 112 | # Grid-to-Particle (G2P) Transfers 113 | # ANCHOR: g2p 114 | @ti.kernel 115 | def grid_to_particle_transfer(): 116 | for p in range(N_particles): 117 | base = (x[p] / dx - 0.5).cast(int) 118 | fx = x[p] / dx - base.cast(float) 119 | # quadratic B-spline interpolating functions (Section 26.2) 120 | w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2] 121 | # gradient of the interpolating function (Section 26.2) 122 | dw_dx = [fx - 1.5, 2 * (1.0 - fx), fx - 0.5] 123 | 124 | new_v = ti.Vector.zero(float, 2) 125 | v_grad_wT = ti.Matrix.zero(float, 2, 2) 126 | for i in ti.static(range(3)): 127 | for j in ti.static(range(3)): 128 | offset = ti.Vector([i, j]) 129 | weight = w[i][0] * w[j][1] 130 | grad_weight = ti.Vector([(1. / dx) * dw_dx[i][0] * w[j][1], 131 | w[i][0] * (1. / dx) * dw_dx[j][1]]) 132 | 133 | new_v += weight * grid_v[base + offset] 134 | v_grad_wT += grid_v[base + offset].outer_product(grad_weight) 135 | 136 | v[p] = new_v 137 | F[p] = (ti.Matrix.identity(float, 2) + dt * v_grad_wT) @ F[p] 138 | # ANCHOR_END: g2p 139 | 140 | # Deformation Gradient and Particle State Update 141 | # ANCHOR: particle_update 142 | @ti.kernel 143 | def update_particle_state(): 144 | for p in range(N_particles): 145 | x[p] += dt * v[p] # advection 146 | # ANCHOR_END: particle_update 147 | 148 | # ANCHOR: time_step 149 | def step(): 150 | # a single time step of the Material Point Method (MPM) simulation 151 | reset_grid() 152 | particle_to_grid_transfer() 153 | update_grid() 154 | grid_to_particle_transfer() 155 | update_particle_state() 156 | # ANCHOR_END: time_step 157 | 158 | ################################ 159 | # Main 160 | ################################ 161 | gui = ti.GUI("2D MPM Elasticity", res = 512, background_color = 0xFFFFFF) 162 | while True: 163 | for s in range(50): step() 164 | 165 | gui.circles(x.to_numpy()[:len(box1_samples)], radius=3, color=0xFF0000) 166 | gui.circles(x.to_numpy()[len(box1_samples):], radius=3, color=0x0000FF) 167 | gui.show() -------------------------------------------------------------------------------- /11_mpm_sand/readme.md: -------------------------------------------------------------------------------- 1 | # 2D Sand with a Sphere Collider 2 | 3 | A 2D sand block falling onto a static red sphere collider simulated by Material Point Method (MPM). The sand undergoes irreversible deformation and splashing upon impact, demonstrating granular flow and frictional boundary response. 4 | 5 | ![results](results.gif) 6 | 7 | ## Dependencies 8 | ``` 9 | pip install numpy taichi 10 | ``` 11 | 12 | ## Run 13 | ``` 14 | python simulator.py 15 | ``` -------------------------------------------------------------------------------- /11_mpm_sand/results.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/phys-sim-book/solid-sim-tutorial/f9d4811b2f95bdf7c189669fb9b614b8baab6fcf/11_mpm_sand/results.gif -------------------------------------------------------------------------------- /1_mass_spring/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /1_mass_spring/MassSpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | def val(x, e, l2, k): 5 | sum = 0.0 6 | for i in range(0, len(e)): 7 | diff = x[e[i][0]] - x[e[i][1]] 8 | sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2 9 | return sum 10 | 11 | def grad(x, e, l2, k): 12 | g = np.array([[0.0, 0.0]] * len(x)) 13 | for i in range(0, len(e)): 14 | diff = x[e[i][0]] - x[e[i][1]] 15 | g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff 16 | g[e[i][0]] += g_diff 17 | g[e[i][1]] -= g_diff 18 | return g 19 | 20 | def hess(x, e, l2, k): 21 | IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))] 22 | for i in range(0, len(e)): 23 | diff = x[e[i][0]] - x[e[i][1]] 24 | H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2)) 25 | H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]])) 26 | # add to global matrix 27 | for nI in range(0, 2): 28 | for nJ in range(0, 2): 29 | indStart = i * 16 + (nI * 2 + nJ) * 4 30 | for r in range(0, 2): 31 | for c in range(0, 2): 32 | IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r 33 | IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c 34 | IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c] 35 | return IJV -------------------------------------------------------------------------------- /1_mass_spring/readme.md: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | An initially stretched square is simulated with mass-spring elasticity potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /1_mass_spring/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | k = 1e5 # spring stiffness 14 | initial_stretch = 1.4 15 | n_seg = 4 # num of segments per side of the square 16 | h = 0.004 # time step size in s 17 | 18 | # initialize simulation 19 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 20 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 21 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 22 | # rest length squared 23 | l2 = [] 24 | for i in range(0, len(e)): 25 | diff = x[e[i][0]] - x[e[i][1]] 26 | l2.append(diff.dot(diff)) 27 | k = [k] * len(e) # spring stiffness 28 | # apply initial stretch horizontally 29 | for i in range(0, len(x)): 30 | x[i][0] *= initial_stretch 31 | 32 | # simulation with visualization 33 | resolution = np.array([900, 900]) 34 | offset = resolution / 2 35 | scale = 200 36 | def screen_projection(x): 37 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 38 | 39 | time_step = 0 40 | square_mesh.write_to_file(time_step, x, n_seg) 41 | screen = pygame.display.set_mode(resolution) 42 | running = True 43 | while running: 44 | # run until the user asks to quit 45 | for event in pygame.event.get(): 46 | if event.type == pygame.QUIT: 47 | running = False 48 | 49 | print('### Time step', time_step, '###') 50 | 51 | # fill the background and draw the square 52 | screen.fill((255, 255, 255)) 53 | for eI in e: 54 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 55 | for xI in x: 56 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 57 | 58 | pygame.display.flip() # flip the display 59 | 60 | # step forward simulation and wait for screen refresh 61 | [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, h, 1e-2) 62 | time_step += 1 63 | pygame.time.wait(int(h * 1000)) 64 | square_mesh.write_to_file(time_step, x, n_seg) 65 | 66 | pygame.quit() -------------------------------------------------------------------------------- /1_mass_spring/square_mesh.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: generate 2 | import numpy as np 3 | import os 4 | 5 | def generate(side_length, n_seg): 6 | # sample nodes uniformly on a square 7 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 8 | step = side_length / n_seg 9 | for i in range(0, n_seg + 1): 10 | for j in range(0, n_seg + 1): 11 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 12 | 13 | # connect the nodes with edges 14 | e = [] 15 | # horizontal edges 16 | for i in range(0, n_seg): 17 | for j in range(0, n_seg + 1): 18 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 19 | # vertical edges 20 | for i in range(0, n_seg + 1): 21 | for j in range(0, n_seg): 22 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 23 | # diagonals 24 | for i in range(0, n_seg): 25 | for j in range(0, n_seg): 26 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 27 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 28 | 29 | return [x, e] 30 | # ANCHOR_END: generate 31 | 32 | # ANCHOR: write_to_file 33 | def write_to_file(frameNum, x, n_seg): 34 | # Check if 'output' directory exists; if not, create it 35 | if not os.path.exists('output'): 36 | os.makedirs('output') 37 | 38 | # create obj file 39 | filename = f"output/{frameNum}.obj" 40 | with open(filename, 'w') as f: 41 | # write vertex coordinates 42 | for row in x: 43 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 44 | # write vertex indices for each triangle 45 | for i in range(0, n_seg): 46 | for j in range(0, n_seg): 47 | #NOTE: each cell is exported as 2 triangles for rendering 48 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n") 49 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n") 50 | # ANCHOR_END: write_to_file -------------------------------------------------------------------------------- /1_mass_spring/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import MassSpringEnergy 11 | 12 | def step_forward(x, e, v, m, l2, k, h, tol): 13 | x_tilde = x + v * h # implicit Euler predictive position 14 | x_n = copy.deepcopy(x) 15 | 16 | # Newton loop 17 | iter = 0 18 | E_last = IP_val(x, e, x_tilde, m, l2, k, h) 19 | p = search_dir(x, e, x_tilde, m, l2, k, h) 20 | while LA.norm(p, inf) / h > tol: 21 | print('Iteration', iter, ':') 22 | print('residual =', LA.norm(p, inf) / h) 23 | 24 | # line search 25 | alpha = 1 26 | while IP_val(x + alpha * p, e, x_tilde, m, l2, k, h) > E_last: 27 | alpha /= 2 28 | print('step size =', alpha) 29 | 30 | x += alpha * p 31 | E_last = IP_val(x, e, x_tilde, m, l2, k, h) 32 | p = search_dir(x, e, x_tilde, m, l2, k, h) 33 | iter += 1 34 | 35 | v = (x - x_n) / h # implicit Euler velocity update 36 | return [x, v] 37 | 38 | def IP_val(x, e, x_tilde, m, l2, k, h): 39 | return InertiaEnergy.val(x, x_tilde, m) + h * h * MassSpringEnergy.val(x, e, l2, k) # implicit Euler 40 | 41 | def IP_grad(x, e, x_tilde, m, l2, k, h): 42 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * MassSpringEnergy.grad(x, e, l2, k) # implicit Euler 43 | 44 | def IP_hess(x, e, x_tilde, m, l2, k, h): 45 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 46 | IJV_MS = MassSpringEnergy.hess(x, e, l2, k) 47 | IJV_MS[2] *= h * h # implicit Euler 48 | IJV = np.append(IJV_In, IJV_MS, axis=1) 49 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 50 | return H 51 | 52 | def search_dir(x, e, x_tilde, m, l2, k, h): 53 | projected_hess = IP_hess(x, e, x_tilde, m, l2, k, h) 54 | reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, h).reshape(len(x) * 2, 1) 55 | return spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2) -------------------------------------------------------------------------------- /1_mass_spring/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | def make_PSD(hess): 5 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 6 | # set all negative Eigenvalues to 0 7 | for i in range(0, len(lam)): 8 | lam[i] = max(0, lam[i]) 9 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) -------------------------------------------------------------------------------- /2_dirichlet/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /2_dirichlet/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /2_dirichlet/MassSpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | def val(x, e, l2, k): 5 | sum = 0.0 6 | for i in range(0, len(e)): 7 | diff = x[e[i][0]] - x[e[i][1]] 8 | sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2 9 | return sum 10 | 11 | def grad(x, e, l2, k): 12 | g = np.array([[0.0, 0.0]] * len(x)) 13 | for i in range(0, len(e)): 14 | diff = x[e[i][0]] - x[e[i][1]] 15 | g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff 16 | g[e[i][0]] += g_diff 17 | g[e[i][1]] -= g_diff 18 | return g 19 | 20 | def hess(x, e, l2, k): 21 | IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))] 22 | for i in range(0, len(e)): 23 | diff = x[e[i][0]] - x[e[i][1]] 24 | H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2)) 25 | H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]])) 26 | # add to global matrix 27 | for nI in range(0, 2): 28 | for nJ in range(0, 2): 29 | indStart = i * 16 + (nI * 2 + nJ) * 4 30 | for r in range(0, 2): 31 | for c in range(0, 2): 32 | IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r 33 | IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c 34 | IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c] 35 | return IJV -------------------------------------------------------------------------------- /2_dirichlet/readme.md: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | A square hanging under gravity with its right and left top nodes fixed is simulated with mass-spring elasticity potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /2_dirichlet/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | k = 1e3 # spring stiffness 14 | n_seg = 4 # num of segments per side of the square 15 | h = 0.02 # time step size in s 16 | # ANCHOR: DBC_def 17 | DBC = [n_seg, (n_seg + 1) * (n_seg + 1) - 1] # fix the left and right top nodes 18 | # ANCHOR_END: DBC_def 19 | 20 | # initialize simulation 21 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 22 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 23 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 24 | # rest length squared 25 | l2 = [] 26 | for i in range(0, len(e)): 27 | diff = x[e[i][0]] - x[e[i][1]] 28 | l2.append(diff.dot(diff)) 29 | k = [k] * len(e) # spring stiffness 30 | # ANCHOR: DBC_mask 31 | # identify whether a node is Dirichlet 32 | is_DBC = [False] * len(x) 33 | for i in DBC: 34 | is_DBC[i] = True 35 | # ANCHOR_END: DBC_mask 36 | # simulation with visualization 37 | resolution = np.array([900, 900]) 38 | offset = resolution / 2 39 | scale = 200 40 | def screen_projection(x): 41 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 42 | 43 | time_step = 0 44 | square_mesh.write_to_file(time_step, x, n_seg) 45 | screen = pygame.display.set_mode(resolution) 46 | running = True 47 | while running: 48 | # run until the user asks to quit 49 | for event in pygame.event.get(): 50 | if event.type == pygame.QUIT: 51 | running = False 52 | 53 | print('### Time step', time_step, '###') 54 | 55 | # fill the background and draw the square 56 | screen.fill((255, 255, 255)) 57 | for eI in e: 58 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 59 | for xI in x: 60 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 61 | 62 | pygame.display.flip() # flip the display 63 | 64 | # step forward simulation and wait for screen refresh 65 | [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, is_DBC, h, 1e-2) 66 | time_step += 1 67 | pygame.time.wait(int(h * 1000)) 68 | square_mesh.write_to_file(time_step, x, n_seg) 69 | 70 | pygame.quit() -------------------------------------------------------------------------------- /2_dirichlet/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with edges 13 | e = [] 14 | # horizontal edges 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg + 1): 17 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 18 | # vertical edges 19 | for i in range(0, n_seg + 1): 20 | for j in range(0, n_seg): 21 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 22 | # diagonals 23 | for i in range(0, n_seg): 24 | for j in range(0, n_seg): 25 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 26 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 27 | 28 | return [x, e] 29 | 30 | def write_to_file(frameNum, x, n_seg): 31 | # Check if 'output' directory exists; if not, create it 32 | if not os.path.exists('output'): 33 | os.makedirs('output') 34 | 35 | # create obj file 36 | filename = f"output/{frameNum}.obj" 37 | with open(filename, 'w') as f: 38 | # write vertex coordinates 39 | for row in x: 40 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 41 | # write vertex indices for each triangle 42 | for i in range(0, n_seg): 43 | for j in range(0, n_seg): 44 | #NOTE: each cell is exported as 2 triangles for rendering 45 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n") 46 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n") -------------------------------------------------------------------------------- /2_dirichlet/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import MassSpringEnergy 11 | import GravityEnergy 12 | 13 | def step_forward(x, e, v, m, l2, k, is_DBC, h, tol): 14 | x_tilde = x + v * h # implicit Euler predictive position 15 | x_n = copy.deepcopy(x) 16 | 17 | # Newton loop 18 | iter = 0 19 | E_last = IP_val(x, e, x_tilde, m, l2, k, h) 20 | p = search_dir(x, e, x_tilde, m, l2, k, is_DBC, h) 21 | while LA.norm(p, inf) / h > tol: 22 | print('Iteration', iter, ':') 23 | print('residual =', LA.norm(p, inf) / h) 24 | 25 | # line search 26 | alpha = 1 27 | while IP_val(x + alpha * p, e, x_tilde, m, l2, k, h) > E_last: 28 | alpha /= 2 29 | print('step size =', alpha) 30 | 31 | x += alpha * p 32 | E_last = IP_val(x, e, x_tilde, m, l2, k, h) 33 | p = search_dir(x, e, x_tilde, m, l2, k, is_DBC, h) 34 | iter += 1 35 | 36 | v = (x - x_n) / h # implicit Euler velocity update 37 | return [x, v] 38 | 39 | # ANCHOR: ADDING_GRAVITY 40 | def IP_val(x, e, x_tilde, m, l2, k, h): 41 | return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m)) # implicit Euler 42 | 43 | def IP_grad(x, e, x_tilde, m, l2, k, h): 44 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m)) # implicit Euler 45 | # ANCHOR_END: ADDING_GRAVITY 46 | 47 | def IP_hess(x, e, x_tilde, m, l2, k, h): 48 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 49 | IJV_MS = MassSpringEnergy.hess(x, e, l2, k) 50 | IJV_MS[2] *= h * h # implicit Euler 51 | IJV = np.append(IJV_In, IJV_MS, axis=1) 52 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 53 | return H 54 | 55 | # ANCHOR: search_dir 56 | def search_dir(x, e, x_tilde, m, l2, k, is_DBC, h): 57 | projected_hess = IP_hess(x, e, x_tilde, m, l2, k, h) 58 | reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, h).reshape(len(x) * 2, 1) 59 | # eliminate DOF by modifying gradient and Hessian for DBC: 60 | for i, j in zip(*projected_hess.nonzero()): 61 | if is_DBC[int(i / 2)] | is_DBC[int(j / 2)]: 62 | projected_hess[i, j] = (i == j) 63 | for i in range(0, len(x)): 64 | if is_DBC[i]: 65 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 66 | return spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2) 67 | #ANCHOR_END: search_dir -------------------------------------------------------------------------------- /2_dirichlet/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | def make_PSD(hess): 5 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 6 | # set all negative Eigenvalues to 0 7 | for i in range(0, len(lam)): 8 | lam[i] = max(0, lam[i]) 9 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) -------------------------------------------------------------------------------- /3_contact/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: val_grad_hess 2 | import math 3 | import numpy as np 4 | 5 | dhat = 0.01 6 | kappa = 1e5 7 | 8 | def val(x, y_ground, contact_area): 9 | sum = 0.0 10 | for i in range(0, len(x)): 11 | d = x[i][1] - y_ground 12 | if d < dhat: 13 | s = d / dhat 14 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 15 | return sum 16 | 17 | def grad(x, y_ground, contact_area): 18 | g = np.array([[0.0, 0.0]] * len(x)) 19 | for i in range(0, len(x)): 20 | d = x[i][1] - y_ground 21 | if d < dhat: 22 | s = d / dhat 23 | g[i][1] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 24 | return g 25 | 26 | def hess(x, y_ground, contact_area): 27 | IJV = [[0] * len(x), [0] * len(x), np.array([0.0] * len(x))] 28 | for i in range(0, len(x)): 29 | IJV[0][i] = i * 2 + 1 30 | IJV[1][i] = i * 2 + 1 31 | d = x[i][1] - y_ground 32 | if d < dhat: 33 | IJV[2][i] = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) 34 | else: 35 | IJV[2][i] = 0.0 36 | return IJV 37 | # ANCHOR_END: val_grad_hess 38 | 39 | # ANCHOR: init_step_size 40 | def init_step_size(x, y_ground, p): 41 | alpha = 1 42 | for i in range(0, len(x)): 43 | if p[i][1] < 0: 44 | alpha = min(alpha, 0.9 * (y_ground - x[i][1]) / p[i][1]) 45 | return alpha 46 | # ANCHOR_END: init_step_size -------------------------------------------------------------------------------- /3_contact/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /3_contact/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /3_contact/MassSpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | def val(x, e, l2, k): 5 | sum = 0.0 6 | for i in range(0, len(e)): 7 | diff = x[e[i][0]] - x[e[i][1]] 8 | sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2 9 | return sum 10 | 11 | def grad(x, e, l2, k): 12 | g = np.array([[0.0, 0.0]] * len(x)) 13 | for i in range(0, len(e)): 14 | diff = x[e[i][0]] - x[e[i][1]] 15 | g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff 16 | g[e[i][0]] += g_diff 17 | g[e[i][1]] -= g_diff 18 | return g 19 | 20 | def hess(x, e, l2, k): 21 | IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))] 22 | for i in range(0, len(e)): 23 | diff = x[e[i][0]] - x[e[i][1]] 24 | H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2)) 25 | H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]])) 26 | # add to global matrix 27 | for nI in range(0, 2): 28 | for nJ in range(0, 2): 29 | indStart = i * 16 + (nI * 2 + nJ) * 4 30 | for r in range(0, 2): 31 | for c in range(0, 2): 32 | IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r 33 | IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c 34 | IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c] 35 | return IJV -------------------------------------------------------------------------------- /3_contact/readme.md: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | A square falling onto the ground under gravity is simulated with mass-spring elasticity potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /3_contact/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | k = 2e4 # spring stiffness 14 | n_seg = 4 # num of segments per side of the square 15 | h = 0.01 # time step size in s 16 | DBC = [] # no nodes need to be fixed 17 | y_ground = -1 # height of the planar ground 18 | 19 | # initialize simulation 20 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 21 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 22 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 23 | # rest length squared 24 | l2 = [] 25 | for i in range(0, len(e)): 26 | diff = x[e[i][0]] - x[e[i][1]] 27 | l2.append(diff.dot(diff)) 28 | k = [k] * len(e) # spring stiffness 29 | # identify whether a node is Dirichlet 30 | is_DBC = [False] * len(x) 31 | for i in DBC: 32 | is_DBC[i] = True 33 | # ANCHOR: contact_area 34 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 35 | # ANCHOR_END: contact_area 36 | 37 | # simulation with visualization 38 | resolution = np.array([900, 900]) 39 | offset = resolution / 2 40 | scale = 200 41 | def screen_projection(x): 42 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 43 | 44 | time_step = 0 45 | square_mesh.write_to_file(time_step, x, n_seg) 46 | screen = pygame.display.set_mode(resolution) 47 | running = True 48 | while running: 49 | # run until the user asks to quit 50 | for event in pygame.event.get(): 51 | if event.type == pygame.QUIT: 52 | running = False 53 | 54 | print('### Time step', time_step, '###') 55 | 56 | # fill the background and draw the square 57 | screen.fill((255, 255, 255)) 58 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([-2, y_ground]), screen_projection([2, y_ground])) # ground 59 | for eI in e: 60 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 61 | for xI in x: 62 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 63 | 64 | pygame.display.flip() # flip the display 65 | 66 | # step forward simulation and wait for screen refresh 67 | [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, y_ground, contact_area, is_DBC, h, 1e-2) 68 | time_step += 1 69 | pygame.time.wait(int(h * 1000)) 70 | square_mesh.write_to_file(time_step, x, n_seg) 71 | 72 | pygame.quit() -------------------------------------------------------------------------------- /3_contact/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with edges 13 | e = [] 14 | # horizontal edges 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg + 1): 17 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 18 | # vertical edges 19 | for i in range(0, n_seg + 1): 20 | for j in range(0, n_seg): 21 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 22 | # diagonals 23 | for i in range(0, n_seg): 24 | for j in range(0, n_seg): 25 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 26 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 27 | 28 | return [x, e] 29 | 30 | def write_to_file(frameNum, x, n_seg): 31 | # Check if 'output' directory exists; if not, create it 32 | if not os.path.exists('output'): 33 | os.makedirs('output') 34 | 35 | # create obj file 36 | filename = f"output/{frameNum}.obj" 37 | with open(filename, 'w') as f: 38 | # write vertex coordinates 39 | for row in x: 40 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 41 | # write vertex indices for each triangle 42 | for i in range(0, n_seg): 43 | for j in range(0, n_seg): 44 | #NOTE: each cell is exported as 2 triangles for rendering 45 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n") 46 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n") -------------------------------------------------------------------------------- /3_contact/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import MassSpringEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | 14 | def step_forward(x, e, v, m, l2, k, y_ground, contact_area, is_DBC, h, tol): 15 | x_tilde = x + v * h # implicit Euler predictive position 16 | x_n = copy.deepcopy(x) 17 | 18 | # Newton loop 19 | iter = 0 20 | E_last = IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h) 21 | p = search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h) 22 | while LA.norm(p, inf) / h > tol: 23 | print('Iteration', iter, ':') 24 | print('residual =', LA.norm(p, inf) / h) 25 | 26 | # ANCHOR: filter_ls 27 | # filter line search 28 | alpha = BarrierEnergy.init_step_size(x, y_ground, p) # avoid interpenetration and tunneling 29 | while IP_val(x + alpha * p, e, x_tilde, m, l2, k, y_ground, contact_area, h) > E_last: 30 | alpha /= 2 31 | # ANCHOR_END: filter_ls 32 | print('step size =', alpha) 33 | 34 | x += alpha * p 35 | E_last = IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h) 36 | p = search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h) 37 | iter += 1 38 | 39 | v = (x - x_n) / h # implicit Euler velocity update 40 | return [x, v] 41 | 42 | def IP_val(x, e, x_tilde, m, l2, k, y_ground, contact_area, h): 43 | return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, y_ground, contact_area)) # implicit Euler 44 | 45 | def IP_grad(x, e, x_tilde, m, l2, k, y_ground, contact_area, h): 46 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, y_ground, contact_area)) # implicit Euler 47 | 48 | def IP_hess(x, e, x_tilde, m, l2, k, y_ground, contact_area, h): 49 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 50 | IJV_MS = MassSpringEnergy.hess(x, e, l2, k) 51 | IJV_B = BarrierEnergy.hess(x, y_ground, contact_area) 52 | IJV_MS[2] *= h * h # implicit Euler 53 | IJV_B[2] *= h * h # implicit Euler 54 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 55 | IJV = np.append(IJV_In_MS, IJV_B, axis=1) 56 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 57 | return H 58 | 59 | def search_dir(x, e, x_tilde, m, l2, k, y_ground, contact_area, is_DBC, h): 60 | projected_hess = IP_hess(x, e, x_tilde, m, l2, k, y_ground, contact_area, h) 61 | reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, y_ground, contact_area, h).reshape(len(x) * 2, 1) 62 | # eliminate DOF by modifying gradient and Hessian for DBC: 63 | for i, j in zip(*projected_hess.nonzero()): 64 | if is_DBC[int(i / 2)] | is_DBC[int(j / 2)]: 65 | projected_hess[i, j] = (i == j) 66 | for i in range(0, len(x)): 67 | if is_DBC[i]: 68 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 69 | return spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2) -------------------------------------------------------------------------------- /3_contact/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | def make_PSD(hess): 5 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 6 | # set all negative Eigenvalues to 0 7 | for i in range(0, len(lam)): 8 | lam[i] = max(0, lam[i]) 9 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) -------------------------------------------------------------------------------- /4_friction/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: slope_barrier 2 | import math 3 | import numpy as np 4 | 5 | dhat = 0.01 6 | kappa = 1e5 7 | 8 | def val(x, n, o, contact_area): 9 | sum = 0.0 10 | for i in range(0, len(x)): 11 | d = n.dot(x[i] - o) 12 | if d < dhat: 13 | s = d / dhat 14 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 15 | return sum 16 | 17 | def grad(x, n, o, contact_area): 18 | g = np.array([[0.0, 0.0]] * len(x)) 19 | for i in range(0, len(x)): 20 | d = n.dot(x[i] - o) 21 | if d < dhat: 22 | s = d / dhat 23 | g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 24 | return g 25 | 26 | def hess(x, n, o, contact_area): 27 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 28 | for i in range(0, len(x)): 29 | d = n.dot(x[i] - o) 30 | if d < dhat: 31 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 32 | for c in range(0, 2): 33 | for r in range(0, 2): 34 | IJV[0].append(i * 2 + r) 35 | IJV[1].append(i * 2 + c) 36 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 37 | return IJV 38 | # ANCHOR_END: slope_barrier 39 | 40 | # ANCHOR: init_step_size 41 | def init_step_size(x, n, o, p): 42 | alpha = 1 43 | for i in range(0, len(x)): 44 | p_n = p[i].dot(n) 45 | if p_n < 0: 46 | alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n) 47 | return alpha 48 | # ANCHOR_END: init_step_size 49 | 50 | # ANCHOR: compute_mu_lambda 51 | def compute_mu_lambda(x, n, o, contact_area, mu): 52 | mu_lambda = np.array([0.0] * len(x)) 53 | for i in range(0, len(x)): 54 | d = n.dot(x[i] - o) 55 | if d < dhat: 56 | s = d / dhat 57 | mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 58 | return mu_lambda 59 | # ANCHOR_END: compute_mu_lambda -------------------------------------------------------------------------------- /4_friction/FrictionEnergy.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: f_terms 2 | import numpy as np 3 | import utils 4 | 5 | epsv = 1e-3 6 | 7 | def f0(vbarnorm, epsv, hhat): 8 | if vbarnorm >= epsv: 9 | return vbarnorm * hhat 10 | else: 11 | vbarnormhhat = vbarnorm * hhat 12 | epsvhhat = epsv * hhat 13 | return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0 14 | 15 | def f1_div_vbarnorm(vbarnorm, epsv): 16 | if vbarnorm >= epsv: 17 | return 1.0 / vbarnorm 18 | else: 19 | return (-vbarnorm + 2.0 * epsv) / (epsv * epsv) 20 | 21 | def f_hess_term(vbarnorm, epsv): 22 | if vbarnorm >= epsv: 23 | return -1.0 / (vbarnorm * vbarnorm) 24 | else: 25 | return -1.0 / (epsv * epsv) 26 | # ANCHOR_END: f_terms 27 | 28 | # ANCHOR: val_grad_hess 29 | def val(v, mu_lambda, hhat, n): 30 | sum = 0.0 31 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 32 | for i in range(0, len(v)): 33 | if mu_lambda[i] > 0: 34 | vbar = np.transpose(T).dot(v[i]) 35 | sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat) 36 | return sum 37 | 38 | def grad(v, mu_lambda, hhat, n): 39 | g = np.array([[0.0, 0.0]] * len(v)) 40 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 41 | for i in range(0, len(v)): 42 | if mu_lambda[i] > 0: 43 | vbar = np.transpose(T).dot(v[i]) 44 | g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 45 | return g 46 | 47 | def hess(v, mu_lambda, hhat, n): 48 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 49 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 50 | for i in range(0, len(v)): 51 | if mu_lambda[i] > 0: 52 | vbar = np.transpose(T).dot(v[i]) 53 | vbarnorm = np.linalg.norm(vbar) 54 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 55 | if vbarnorm != 0: 56 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 57 | local_hess = mu_lambda[i] * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 58 | for c in range(0, 2): 59 | for r in range(0, 2): 60 | IJV[0].append(i * 2 + r) 61 | IJV[1].append(i * 2 + c) 62 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 63 | return IJV 64 | # ANCHOR_END: val_grad_hess -------------------------------------------------------------------------------- /4_friction/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /4_friction/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /4_friction/MassSpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | def val(x, e, l2, k): 5 | sum = 0.0 6 | for i in range(0, len(e)): 7 | diff = x[e[i][0]] - x[e[i][1]] 8 | sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2 9 | return sum 10 | 11 | def grad(x, e, l2, k): 12 | g = np.array([[0.0, 0.0]] * len(x)) 13 | for i in range(0, len(e)): 14 | diff = x[e[i][0]] - x[e[i][1]] 15 | g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff 16 | g[e[i][0]] += g_diff 17 | g[e[i][1]] -= g_diff 18 | return g 19 | 20 | def hess(x, e, l2, k): 21 | IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))] 22 | for i in range(0, len(e)): 23 | diff = x[e[i][0]] - x[e[i][1]] 24 | H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2)) 25 | H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]])) 26 | # add to global matrix 27 | for nI in range(0, 2): 28 | for nJ in range(0, 2): 29 | indStart = i * 16 + (nI * 2 + nJ) * 4 30 | for r in range(0, 2): 31 | for c in range(0, 2): 32 | IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r 33 | IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c 34 | IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c] 35 | return IJV -------------------------------------------------------------------------------- /4_friction/readme.md: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | A square sliding/residing on a slope under gravity is simulated with mass-spring elasticity potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /4_friction/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | k = 2e4 # spring stiffness 14 | n_seg = 4 # num of segments per side of the square 15 | h = 0.01 # time step size in s 16 | DBC = [] # no nodes need to be fixed 17 | # ANCHOR: slope_setup 18 | ground_n = np.array([0.1, 1.0]) # normal of the slope 19 | ground_n /= np.linalg.norm(ground_n) # normalize ground normal vector just in case 20 | ground_o = np.array([0.0, -1.0]) # a point on the slope 21 | # ANCHOR_END: slope_setup 22 | # ANCHOR: set_mu 23 | mu = 0.11 # friction coefficient of the slope 24 | # ANCHOR_END: set_mu 25 | 26 | # initialize simulation 27 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 28 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 29 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 30 | # rest length squared 31 | l2 = [] 32 | for i in range(0, len(e)): 33 | diff = x[e[i][0]] - x[e[i][1]] 34 | l2.append(diff.dot(diff)) 35 | k = [k] * len(e) # spring stiffness 36 | # identify whether a node is Dirichlet 37 | is_DBC = [False] * len(x) 38 | for i in DBC: 39 | is_DBC[i] = True 40 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 41 | 42 | # simulation with visualization 43 | resolution = np.array([900, 900]) 44 | offset = resolution / 2 45 | scale = 200 46 | def screen_projection(x): 47 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 48 | 49 | time_step = 0 50 | square_mesh.write_to_file(time_step, x, n_seg) 51 | screen = pygame.display.set_mode(resolution) 52 | running = True 53 | while running: 54 | # run until the user asks to quit 55 | for event in pygame.event.get(): 56 | if event.type == pygame.QUIT: 57 | running = False 58 | 59 | print('### Time step', time_step, '###') 60 | 61 | # fill the background and draw the square 62 | screen.fill((255, 255, 255)) 63 | # ANCHOR: slope_vis 64 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 65 | screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]])) # slope 66 | # ANCHOR_END: slope_vis 67 | for eI in e: 68 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 69 | for xI in x: 70 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 71 | 72 | pygame.display.flip() # flip the display 73 | 74 | # step forward simulation and wait for screen refresh 75 | [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, ground_n, ground_o, contact_area, mu, is_DBC, h, 1e-2) 76 | time_step += 1 77 | pygame.time.wait(int(h * 1000)) 78 | square_mesh.write_to_file(time_step, x, n_seg) 79 | 80 | pygame.quit() -------------------------------------------------------------------------------- /4_friction/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with edges 13 | e = [] 14 | # horizontal edges 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg + 1): 17 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 18 | # vertical edges 19 | for i in range(0, n_seg + 1): 20 | for j in range(0, n_seg): 21 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 22 | # diagonals 23 | for i in range(0, n_seg): 24 | for j in range(0, n_seg): 25 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 26 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 27 | 28 | return [x, e] 29 | 30 | def write_to_file(frameNum, x, n_seg): 31 | # Check if 'output' directory exists; if not, create it 32 | if not os.path.exists('output'): 33 | os.makedirs('output') 34 | 35 | # create obj file 36 | filename = f"output/{frameNum}.obj" 37 | with open(filename, 'w') as f: 38 | # write vertex coordinates 39 | for row in x: 40 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 41 | # write vertex indices for each triangle 42 | for i in range(0, n_seg): 43 | for j in range(0, n_seg): 44 | #NOTE: each cell is exported as 2 triangles for rendering 45 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n") 46 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n") -------------------------------------------------------------------------------- /4_friction/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import MassSpringEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | import FrictionEnergy 14 | 15 | # ANCHOR: mu_lambda 16 | def step_forward(x, e, v, m, l2, k, n, o, contact_area, mu, is_DBC, h, tol): 17 | x_tilde = x + v * h # implicit Euler predictive position 18 | x_n = copy.deepcopy(x) 19 | mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, contact_area, mu) # compute mu * lambda for each node using x^n 20 | 21 | # Newton loop 22 | # ANCHOR_END: mu_lambda 23 | iter = 0 24 | E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, h) 25 | p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, h) 26 | while LA.norm(p, inf) / h > tol: 27 | print('Iteration', iter, ':') 28 | print('residual =', LA.norm(p, inf) / h) 29 | 30 | # filter line search 31 | alpha = BarrierEnergy.init_step_size(x, n, o, p) # avoid interpenetration and tunneling 32 | while IP_val(x + alpha * p, e, x_tilde, m, l2, k, n, o, contact_area, (x + alpha * p - x_n) / h, mu_lambda, h) > E_last: 33 | alpha /= 2 34 | print('step size =', alpha) 35 | 36 | x += alpha * p 37 | E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, h) 38 | p = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, h) 39 | iter += 1 40 | 41 | v = (x - x_n) / h # implicit Euler velocity update 42 | return [x, v] 43 | 44 | def IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h): 45 | return InertiaEnergy.val(x, x_tilde, m) + h * h * (MassSpringEnergy.val(x, e, l2, k) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, n, o, contact_area) + FrictionEnergy.val(v, mu_lambda, h, n)) # implicit Euler 46 | 47 | def IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h): 48 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * (MassSpringEnergy.grad(x, e, l2, k) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, n, o, contact_area) + FrictionEnergy.grad(v, mu_lambda, h, n)) # implicit Euler 49 | 50 | def IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h): 51 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 52 | IJV_MS = MassSpringEnergy.hess(x, e, l2, k) 53 | IJV_B = BarrierEnergy.hess(x, n, o, contact_area) 54 | IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n) 55 | IJV_MS[2] *= h * h # implicit Euler 56 | IJV_B[2] *= h * h # implicit Euler 57 | IJV_F[2] *= h * h # implicit Euler 58 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 59 | IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1) 60 | IJV = np.append(IJV_In_MS_B, IJV_F, axis=1) 61 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 62 | return H 63 | 64 | def search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, is_DBC, h): 65 | projected_hess = IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h) 66 | reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, h).reshape(len(x) * 2, 1) 67 | # eliminate DOF by modifying gradient and Hessian for DBC: 68 | for i, j in zip(*projected_hess.nonzero()): 69 | if is_DBC[int(i / 2)] | is_DBC[int(j / 2)]: 70 | projected_hess[i, j] = (i == j) 71 | for i in range(0, len(x)): 72 | if is_DBC[i]: 73 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 74 | return spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2) -------------------------------------------------------------------------------- /4_friction/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | def make_PSD(hess): 5 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 6 | # set all negative Eigenvalues to 0 7 | for i in range(0, len(lam)): 8 | lam[i] = max(0, lam[i]) 9 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) -------------------------------------------------------------------------------- /5_mov_dirichlet/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | dhat = 0.01 5 | kappa = 1e5 6 | 7 | def val(x, n, o, contact_area): 8 | sum = 0.0 9 | # floor: 10 | for i in range(0, len(x)): 11 | d = n.dot(x[i] - o) 12 | if d < dhat: 13 | s = d / dhat 14 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 15 | # ANCHOR: ceiling_val 16 | n = np.array([0.0, -1.0]) 17 | for i in range(0, len(x) - 1): 18 | d = n.dot(x[i] - x[-1]) 19 | if d < dhat: 20 | s = d / dhat 21 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 22 | # ANCHOR_END: ceiling_val 23 | return sum 24 | 25 | def grad(x, n, o, contact_area): 26 | g = np.array([[0.0, 0.0]] * len(x)) 27 | # floor: 28 | for i in range(0, len(x)): 29 | d = n.dot(x[i] - o) 30 | if d < dhat: 31 | s = d / dhat 32 | g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 33 | # ANCHOR: ceiling_grad 34 | n = np.array([0.0, -1.0]) 35 | for i in range(0, len(x) - 1): 36 | d = n.dot(x[i] - x[-1]) 37 | if d < dhat: 38 | s = d / dhat 39 | local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 40 | g[i] += local_grad 41 | g[-1] -= local_grad 42 | # ANCHOR_END: ceiling_grad 43 | return g 44 | 45 | def hess(x, n, o, contact_area): 46 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 47 | # floor: 48 | for i in range(0, len(x)): 49 | d = n.dot(x[i] - o) 50 | if d < dhat: 51 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 52 | for c in range(0, 2): 53 | for r in range(0, 2): 54 | IJV[0].append(i * 2 + r) 55 | IJV[1].append(i * 2 + c) 56 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 57 | # ANCHOR: ceiling_hess 58 | n = np.array([0.0, -1.0]) 59 | for i in range(0, len(x) - 1): 60 | d = n.dot(x[i] - x[-1]) 61 | if d < dhat: 62 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 63 | index = [i, len(x) - 1] 64 | for nI in range(0, 2): 65 | for nJ in range(0, 2): 66 | for c in range(0, 2): 67 | for r in range(0, 2): 68 | IJV[0].append(index[nI] * 2 + r) 69 | IJV[1].append(index[nJ] * 2 + c) 70 | IJV[2] = np.append(IJV[2], ((-1) ** (nI != nJ)) * local_hess[r, c]) 71 | # ANCHOR_END: ceiling_hess 72 | return IJV 73 | 74 | def init_step_size(x, n, o, p): 75 | alpha = 1 76 | # floor: 77 | for i in range(0, len(x)): 78 | p_n = p[i].dot(n) 79 | if p_n < 0: 80 | alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n) 81 | # ANCHOR: ceiling_ccd 82 | n = np.array([0.0, -1.0]) 83 | for i in range(0, len(x) - 1): 84 | p_n = (p[i] - p[-1]).dot(n) 85 | if p_n < 0: 86 | alpha = min(alpha, 0.9 * n.dot(x[i] - x[-1]) / -p_n) 87 | # ANCHOR_END: ceiling_ccd 88 | return alpha 89 | 90 | def compute_mu_lambda(x, n, o, contact_area, mu): 91 | mu_lambda = np.array([0.0] * len(x)) 92 | for i in range(0, len(x)): 93 | d = n.dot(x[i] - o) 94 | if d < dhat: 95 | s = d / dhat 96 | mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 97 | return mu_lambda -------------------------------------------------------------------------------- /5_mov_dirichlet/FrictionEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | epsv = 1e-3 5 | 6 | def f0(vbarnorm, epsv, hhat): 7 | if vbarnorm >= epsv: 8 | return vbarnorm * hhat 9 | else: 10 | vbarnormhhat = vbarnorm * hhat 11 | epsvhhat = epsv * hhat 12 | return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0 13 | 14 | def f1_div_vbarnorm(vbarnorm, epsv): 15 | if vbarnorm >= epsv: 16 | return 1.0 / vbarnorm 17 | else: 18 | return (-vbarnorm + 2.0 * epsv) / (epsv * epsv) 19 | 20 | def f_hess_term(vbarnorm, epsv): 21 | if vbarnorm >= epsv: 22 | return -1.0 / (vbarnorm * vbarnorm) 23 | else: 24 | return -1.0 / (epsv * epsv) 25 | 26 | def val(v, mu_lambda, hhat, n): 27 | sum = 0.0 28 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 29 | for i in range(0, len(v)): 30 | if mu_lambda[i] > 0: 31 | vbar = np.transpose(T).dot(v[i]) 32 | sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat) 33 | return sum 34 | 35 | def grad(v, mu_lambda, hhat, n): 36 | g = np.array([[0.0, 0.0]] * len(v)) 37 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 38 | for i in range(0, len(v)): 39 | if mu_lambda[i] > 0: 40 | vbar = np.transpose(T).dot(v[i]) 41 | g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 42 | return g 43 | 44 | def hess(v, mu_lambda, hhat, n): 45 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 46 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 47 | for i in range(0, len(v)): 48 | if mu_lambda[i] > 0: 49 | vbar = np.transpose(T).dot(v[i]) 50 | vbarnorm = np.linalg.norm(vbar) 51 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 52 | if vbarnorm != 0: 53 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 54 | local_hess = mu_lambda[i] * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 55 | for c in range(0, 2): 56 | for r in range(0, 2): 57 | IJV[0].append(i * 2 + r) 58 | IJV[1].append(i * 2 + c) 59 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 60 | return IJV -------------------------------------------------------------------------------- /5_mov_dirichlet/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /5_mov_dirichlet/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /5_mov_dirichlet/MassSpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | def val(x, e, l2, k): 5 | sum = 0.0 6 | for i in range(0, len(e)): 7 | diff = x[e[i][0]] - x[e[i][1]] 8 | sum += l2[i] * 0.5 * k[i] * (diff.dot(diff) / l2[i] - 1) ** 2 9 | return sum 10 | 11 | def grad(x, e, l2, k): 12 | g = np.array([[0.0, 0.0]] * len(x)) 13 | for i in range(0, len(e)): 14 | diff = x[e[i][0]] - x[e[i][1]] 15 | g_diff = 2 * k[i] * (diff.dot(diff) / l2[i] - 1) * diff 16 | g[e[i][0]] += g_diff 17 | g[e[i][1]] -= g_diff 18 | return g 19 | 20 | def hess(x, e, l2, k): 21 | IJV = [[0] * (len(e) * 16), [0] * (len(e) * 16), np.array([0.0] * (len(e) * 16))] 22 | for i in range(0, len(e)): 23 | diff = x[e[i][0]] - x[e[i][1]] 24 | H_diff = 2 * k[i] / l2[i] * (2 * np.outer(diff, diff) + (diff.dot(diff) - l2[i]) * np.identity(2)) 25 | H_local = utils.make_PSD(np.block([[H_diff, -H_diff], [-H_diff, H_diff]])) 26 | # add to global matrix 27 | for nI in range(0, 2): 28 | for nJ in range(0, 2): 29 | indStart = i * 16 + (nI * 2 + nJ) * 4 30 | for r in range(0, 2): 31 | for c in range(0, 2): 32 | IJV[0][indStart + r * 2 + c] = e[i][nI] * 2 + r 33 | IJV[1][indStart + r * 2 + c] = e[i][nJ] * 2 + c 34 | IJV[2][indStart + r * 2 + c] = H_local[nI * 2 + r, nJ * 2 + c] 35 | return IJV -------------------------------------------------------------------------------- /5_mov_dirichlet/SpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, m, DBC, DBC_target, k): 4 | sum = 0.0 5 | for i in range(0, len(DBC)): 6 | diff = x[DBC[i]] - DBC_target[i] 7 | sum += 0.5 * k * m[DBC[i]] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, m, DBC, DBC_target, k): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(DBC)): 13 | g[DBC[i]] = k * m[DBC[i]] * (x[DBC[i]] - DBC_target[i]) 14 | return g 15 | 16 | def hess(x, m, DBC, DBC_target, k): 17 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 18 | for i in range(0, len(DBC)): 19 | for d in range(0, 2): 20 | IJV[0].append(DBC[i] * 2 + d) 21 | IJV[1].append(DBC[i] * 2 + d) 22 | IJV[2] = np.append(IJV[2], k * m[DBC[i]]) 23 | return IJV -------------------------------------------------------------------------------- /5_mov_dirichlet/readme.md: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | A square falling onto a ground under gravity and then compressed by a ceiling is simulated with mass-spring elasticity potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /5_mov_dirichlet/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | k = 2e4 # spring stiffness 14 | n_seg = 4 # num of segments per side of the square 15 | h = 0.01 # time step size in s 16 | # ANCHOR: ceiling_dbc_setup 17 | DBC = [(n_seg + 1) * (n_seg + 1)] # dirichlet node index 18 | DBC_v = [np.array([0.0, -0.5])] # dirichlet node velocity 19 | DBC_limit = [np.array([0.0, -0.6])] # dirichlet node limit position 20 | # ANCHOR_END: ceiling_dbc_setup 21 | ground_n = np.array([0.0, 1.0]) # normal of the slope 22 | ground_n /= np.linalg.norm(ground_n) # normalize ground normal vector just in case 23 | ground_o = np.array([0.0, -1.0]) # a point on the slope 24 | mu = 0.11 # friction coefficient of the slope 25 | 26 | # initialize simulation 27 | # ANCHOR: ceiling_dof 28 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 29 | x = np.append(x, [[0.0, side_len * 0.6]], axis=0) # ceil origin (with normal [0.0, -1.0]) 30 | # ANCHOR_END: ceiling_dof 31 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 32 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 33 | # rest length squared 34 | l2 = [] 35 | for i in range(0, len(e)): 36 | diff = x[e[i][0]] - x[e[i][1]] 37 | l2.append(diff.dot(diff)) 38 | k = [k] * len(e) # spring stiffness 39 | # identify whether a node is Dirichlet 40 | is_DBC = [False] * len(x) 41 | for i in DBC: 42 | is_DBC[i] = True 43 | DBC_stiff = [1000] # DBC stiffness, adjusted and warm-started across time steps 44 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 45 | 46 | # simulation with visualization 47 | resolution = np.array([900, 900]) 48 | offset = resolution / 2 49 | scale = 200 50 | def screen_projection(x): 51 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 52 | 53 | time_step = 0 54 | square_mesh.write_to_file(time_step, x, n_seg) 55 | screen = pygame.display.set_mode(resolution) 56 | running = True 57 | while running: 58 | # run until the user asks to quit 59 | for event in pygame.event.get(): 60 | if event.type == pygame.QUIT: 61 | running = False 62 | 63 | print('### Time step', time_step, '###') 64 | 65 | # fill the background and draw the square 66 | screen.fill((255, 255, 255)) 67 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 68 | screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]])) # ground 69 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([x[-1][0] + 3.0, x[-1][1]]), 70 | screen_projection([x[-1][0] - 3.0, x[-1][1]])) # ceil 71 | for eI in e: 72 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 73 | for xId in range(0, len(x) - 1): 74 | xI = x[xId] 75 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 76 | 77 | pygame.display.flip() # flip the display 78 | 79 | # step forward simulation and wait for screen refresh 80 | [x, v] = time_integrator.step_forward(x, e, v, m, l2, k, ground_n, ground_o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, 1e-2) 81 | time_step += 1 82 | pygame.time.wait(int(h * 1000)) 83 | square_mesh.write_to_file(time_step, x, n_seg) 84 | 85 | pygame.quit() -------------------------------------------------------------------------------- /5_mov_dirichlet/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with edges 13 | e = [] 14 | # horizontal edges 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg + 1): 17 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j]) 18 | # vertical edges 19 | for i in range(0, n_seg + 1): 20 | for j in range(0, n_seg): 21 | e.append([i * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 22 | # diagonals 23 | for i in range(0, n_seg): 24 | for j in range(0, n_seg): 25 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 26 | e.append([(i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 27 | 28 | return [x, e] 29 | 30 | def write_to_file(frameNum, x, n_seg): 31 | # Check if 'output' directory exists; if not, create it 32 | if not os.path.exists('output'): 33 | os.makedirs('output') 34 | 35 | # create obj file 36 | filename = f"output/{frameNum}.obj" 37 | with open(filename, 'w') as f: 38 | # write vertex coordinates 39 | for row in x: 40 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 41 | # write vertex indices for each triangle 42 | for i in range(0, n_seg): 43 | for j in range(0, n_seg): 44 | #NOTE: each cell is exported as 2 triangles for rendering 45 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1}\n") 46 | f.write(f"f {i * (n_seg+1) + j + 1} {(i+1) * (n_seg+1) + j+1 + 1} {i * (n_seg+1) + j+1 + 1}\n") -------------------------------------------------------------------------------- /5_mov_dirichlet/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import MassSpringEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | import FrictionEnergy 14 | import SpringEnergy 15 | 16 | def step_forward(x, e, v, m, l2, k, n, o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, tol): 17 | x_tilde = x + v * h # implicit Euler predictive position 18 | x_n = copy.deepcopy(x) 19 | mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, contact_area, mu) # compute mu * lambda for each node using x^n 20 | # ANCHOR: dbc_initialization 21 | DBC_target = [] # target position of each DBC in the current time step 22 | for i in range(0, len(DBC)): 23 | if (DBC_limit[i] - x_n[DBC[i]]).dot(DBC_v[i]) > 0: 24 | DBC_target.append(x_n[DBC[i]] + h * DBC_v[i]) 25 | else: 26 | DBC_target.append(x_n[DBC[i]]) 27 | # ANCHOR_END: dbc_initialization 28 | 29 | # Newton loop 30 | iter = 0 31 | E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 32 | # ANCHOR: convergence_criteria 33 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 34 | while (LA.norm(p, inf) / h > tol) | (sum(DBC_satisfied) != len(DBC)): # also check whether all DBCs are satisfied 35 | print('Iteration', iter, ':') 36 | print('residual =', LA.norm(p, inf) / h) 37 | 38 | if (LA.norm(p, inf) / h <= tol) & (sum(DBC_satisfied) != len(DBC)): 39 | # increase DBC stiffness and recompute energy value record 40 | DBC_stiff[0] *= 2 41 | E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 42 | # ANCHOR_END: convergence_criteria 43 | 44 | # filter line search 45 | alpha = BarrierEnergy.init_step_size(x, n, o, p) # avoid interpenetration and tunneling 46 | while IP_val(x + alpha * p, e, x_tilde, m, l2, k, n, o, contact_area, (x + alpha * p - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) > E_last: 47 | alpha /= 2 48 | print('step size =', alpha) 49 | 50 | x += alpha * p 51 | E_last = IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 52 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 53 | iter += 1 54 | 55 | v = (x - x_n) / h # implicit Euler velocity update 56 | return [x, v] 57 | 58 | def IP_val(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 59 | return InertiaEnergy.val(x, x_tilde, m) + h * h * ( # implicit Euler 60 | MassSpringEnergy.val(x, e, l2, k) + 61 | GravityEnergy.val(x, m) + 62 | BarrierEnergy.val(x, n, o, contact_area) + 63 | FrictionEnergy.val(v, mu_lambda, h, n) 64 | ) + SpringEnergy.val(x, m, DBC, DBC_target, DBC_stiff) 65 | 66 | def IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 67 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * ( # implicit Euler 68 | MassSpringEnergy.grad(x, e, l2, k) + 69 | GravityEnergy.grad(x, m) + 70 | BarrierEnergy.grad(x, n, o, contact_area) + 71 | FrictionEnergy.grad(v, mu_lambda, h, n) 72 | ) + SpringEnergy.grad(x, m, DBC, DBC_target, DBC_stiff) 73 | 74 | def IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 75 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 76 | IJV_MS = MassSpringEnergy.hess(x, e, l2, k) 77 | IJV_B = BarrierEnergy.hess(x, n, o, contact_area) 78 | IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n) 79 | IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff) 80 | IJV_MS[2] *= h * h # implicit Euler 81 | IJV_B[2] *= h * h # implicit Euler 82 | IJV_F[2] *= h * h # implicit Euler 83 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 84 | IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1) 85 | IJV_In_MS_B_F = np.append(IJV_In_MS_B, IJV_F, axis=1) 86 | IJV = np.append(IJV_In_MS_B_F, IJV_S, axis=1) 87 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 88 | return H 89 | 90 | def search_dir(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h): 91 | projected_hess = IP_hess(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h) 92 | reshaped_grad = IP_grad(x, e, x_tilde, m, l2, k, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1) 93 | # ANCHOR: dbc_check 94 | # check whether each DBC is satisfied 95 | DBC_satisfied = [False] * len(x) 96 | for i in range(0, len(DBC)): 97 | if LA.norm(x[DBC[i]] - DBC_target[i]) / h < tol: 98 | DBC_satisfied[DBC[i]] = True 99 | # ANCHOR_END: dbc_check 100 | # ANCHOR: dof_elimination 101 | # eliminate DOF if it's a satisfied DBC by modifying gradient and Hessian for DBC: 102 | for i, j in zip(*projected_hess.nonzero()): 103 | if (is_DBC[int(i / 2)] & DBC_satisfied[int(i / 2)]) | (is_DBC[int(j / 2)] & DBC_satisfied[int(j / 2)]): 104 | projected_hess[i, j] = (i == j) 105 | for i in range(0, len(x)): 106 | if is_DBC[i] & DBC_satisfied[i]: 107 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 108 | return [spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2), DBC_satisfied] 109 | # ANCHOR_END: dof_elimination -------------------------------------------------------------------------------- /5_mov_dirichlet/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | def make_PSD(hess): 5 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 6 | # set all negative Eigenvalues to 0 7 | for i in range(0, len(lam)): 8 | lam[i] = max(0, lam[i]) 9 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) -------------------------------------------------------------------------------- /6_inv_free/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | dhat = 0.01 5 | kappa = 1e5 6 | 7 | def val(x, n, o, contact_area): 8 | sum = 0.0 9 | # floor: 10 | for i in range(0, len(x)): 11 | d = n.dot(x[i] - o) 12 | if d < dhat: 13 | s = d / dhat 14 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 15 | # ceil: 16 | n = np.array([0.0, -1.0]) 17 | for i in range(0, len(x) - 1): 18 | d = n.dot(x[i] - x[-1]) 19 | if d < dhat: 20 | s = d / dhat 21 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 22 | return sum 23 | 24 | def grad(x, n, o, contact_area): 25 | g = np.array([[0.0, 0.0]] * len(x)) 26 | # floor: 27 | for i in range(0, len(x)): 28 | d = n.dot(x[i] - o) 29 | if d < dhat: 30 | s = d / dhat 31 | g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 32 | # ceil: 33 | n = np.array([0.0, -1.0]) 34 | for i in range(0, len(x) - 1): 35 | d = n.dot(x[i] - x[-1]) 36 | if d < dhat: 37 | s = d / dhat 38 | local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 39 | g[i] += local_grad 40 | g[-1] -= local_grad 41 | return g 42 | 43 | def hess(x, n, o, contact_area): 44 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 45 | # floor: 46 | for i in range(0, len(x)): 47 | d = n.dot(x[i] - o) 48 | if d < dhat: 49 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 50 | for c in range(0, 2): 51 | for r in range(0, 2): 52 | IJV[0].append(i * 2 + r) 53 | IJV[1].append(i * 2 + c) 54 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 55 | # ceil: 56 | n = np.array([0.0, -1.0]) 57 | for i in range(0, len(x) - 1): 58 | d = n.dot(x[i] - x[-1]) 59 | if d < dhat: 60 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 61 | index = [i, len(x) - 1] 62 | for nI in range(0, 2): 63 | for nJ in range(0, 2): 64 | for c in range(0, 2): 65 | for r in range(0, 2): 66 | IJV[0].append(index[nI] * 2 + r) 67 | IJV[1].append(index[nJ] * 2 + c) 68 | IJV[2] = np.append(IJV[2], ((-1) ** (nI != nJ)) * local_hess[r, c]) 69 | return IJV 70 | 71 | def init_step_size(x, n, o, p): 72 | alpha = 1 73 | # floor: 74 | for i in range(0, len(x)): 75 | p_n = p[i].dot(n) 76 | if p_n < 0: 77 | alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n) 78 | # ceil: 79 | n = np.array([0.0, -1.0]) 80 | for i in range(0, len(x) - 1): 81 | p_n = (p[i] - p[-1]).dot(n) 82 | if p_n < 0: 83 | alpha = min(alpha, 0.9 * n.dot(x[i] - x[-1]) / -p_n) 84 | return alpha 85 | 86 | def compute_mu_lambda(x, n, o, contact_area, mu): 87 | mu_lambda = np.array([0.0] * len(x)) 88 | for i in range(0, len(x)): 89 | d = n.dot(x[i] - o) 90 | if d < dhat: 91 | s = d / dhat 92 | mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 93 | return mu_lambda -------------------------------------------------------------------------------- /6_inv_free/FrictionEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | epsv = 1e-3 5 | 6 | def f0(vbarnorm, epsv, hhat): 7 | if vbarnorm >= epsv: 8 | return vbarnorm * hhat 9 | else: 10 | vbarnormhhat = vbarnorm * hhat 11 | epsvhhat = epsv * hhat 12 | return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0 13 | 14 | def f1_div_vbarnorm(vbarnorm, epsv): 15 | if vbarnorm >= epsv: 16 | return 1.0 / vbarnorm 17 | else: 18 | return (-vbarnorm + 2.0 * epsv) / (epsv * epsv) 19 | 20 | def f_hess_term(vbarnorm, epsv): 21 | if vbarnorm >= epsv: 22 | return -1.0 / (vbarnorm * vbarnorm) 23 | else: 24 | return -1.0 / (epsv * epsv) 25 | 26 | def val(v, mu_lambda, hhat, n): 27 | sum = 0.0 28 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 29 | for i in range(0, len(v)): 30 | if mu_lambda[i] > 0: 31 | vbar = np.transpose(T).dot(v[i]) 32 | sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat) 33 | return sum 34 | 35 | def grad(v, mu_lambda, hhat, n): 36 | g = np.array([[0.0, 0.0]] * len(v)) 37 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 38 | for i in range(0, len(v)): 39 | if mu_lambda[i] > 0: 40 | vbar = np.transpose(T).dot(v[i]) 41 | g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 42 | return g 43 | 44 | def hess(v, mu_lambda, hhat, n): 45 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 46 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 47 | for i in range(0, len(v)): 48 | if mu_lambda[i] > 0: 49 | vbar = np.transpose(T).dot(v[i]) 50 | vbarnorm = np.linalg.norm(vbar) 51 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 52 | if vbarnorm != 0: 53 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 54 | local_hess = mu_lambda[i] * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 55 | for c in range(0, 2): 56 | for r in range(0, 2): 57 | IJV[0].append(i * 2 + r) 58 | IJV[1].append(i * 2 + c) 59 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 60 | return IJV -------------------------------------------------------------------------------- /6_inv_free/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /6_inv_free/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /6_inv_free/SpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, m, DBC, DBC_target, k): 4 | sum = 0.0 5 | for i in range(0, len(DBC)): 6 | diff = x[DBC[i]] - DBC_target[i] 7 | sum += 0.5 * k * m[DBC[i]] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, m, DBC, DBC_target, k): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(DBC)): 13 | g[DBC[i]] = k * m[DBC[i]] * (x[DBC[i]] - DBC_target[i]) 14 | return g 15 | 16 | def hess(x, m, DBC, DBC_target, k): 17 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 18 | for i in range(0, len(DBC)): 19 | for d in range(0, 2): 20 | IJV[0].append(DBC[i] * 2 + d) 21 | IJV[1].append(DBC[i] * 2 + d) 22 | IJV[2] = np.append(IJV[2], k * m[DBC[i]]) 23 | return IJV -------------------------------------------------------------------------------- /6_inv_free/readme.md: -------------------------------------------------------------------------------- 1 | # Inversion-free Hyperelastic Solids Simulation 2 | 3 | A square falling onto a ground under gravity and then compressed by a ceiling is simulated with an inversion-free hyperelastic potential and implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /6_inv_free/simulator.py: -------------------------------------------------------------------------------- 1 | # FEM Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 1 12 | rho = 1000 # density of square 13 | # ANCHOR: lame_param 14 | E = 1e5 # Young's modulus 15 | nu = 0.4 # Poisson's ratio 16 | # ANCHOR_END: lame_param 17 | n_seg = 4 # num of segments per side of the square 18 | h = 0.01 # time step size in s 19 | DBC = [(n_seg + 1) * (n_seg + 1)] # dirichlet node index 20 | DBC_v = [np.array([0.0, -0.5])] # dirichlet node velocity 21 | DBC_limit = [np.array([0.0, -0.7])] # dirichlet node limit position 22 | ground_n = np.array([0.0, 1.0]) # normal of the slope 23 | ground_n /= np.linalg.norm(ground_n) # normalize ground normal vector just in case 24 | ground_o = np.array([0.0, -1.0]) # a point on the slope 25 | mu = 0.11 # friction coefficient of the slope 26 | 27 | # initialize simulation 28 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and triangle node indices 29 | x = np.append(x, [[0.0, side_len * 0.6]], axis=0) # ceil origin (with normal [0.0, -1.0]) 30 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 31 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 32 | # ANCHOR: elem_precomp 33 | # rest shape basis, volume, and lame parameters 34 | vol = [0.0] * len(e) 35 | IB = [np.array([[0.0, 0.0]] * 2)] * len(e) 36 | for i in range(0, len(e)): 37 | TB = [x[e[i][1]] - x[e[i][0]], x[e[i][2]] - x[e[i][0]]] 38 | vol[i] = np.linalg.det(np.transpose(TB)) / 2 39 | IB[i] = np.linalg.inv(np.transpose(TB)) 40 | mu_lame = [0.5 * E / (1 + nu)] * len(e) 41 | lam = [E * nu / ((1 + nu) * (1 - 2 * nu))] * len(e) 42 | # ANCHOR_END: elem_precomp 43 | # identify whether a node is Dirichlet 44 | is_DBC = [False] * len(x) 45 | for i in DBC: 46 | is_DBC[i] = True 47 | DBC_stiff = [1000] # DBC stiffness, adjusted and warm-started across time steps 48 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 49 | 50 | # simulation with visualization 51 | resolution = np.array([900, 900]) 52 | offset = resolution / 2 53 | scale = 200 54 | def screen_projection(x): 55 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 56 | 57 | time_step = 0 58 | square_mesh.write_to_file(time_step, x, e) 59 | screen = pygame.display.set_mode(resolution) 60 | running = True 61 | while running: 62 | # run until the user asks to quit 63 | for event in pygame.event.get(): 64 | if event.type == pygame.QUIT: 65 | running = False 66 | 67 | print('### Time step', time_step, '###') 68 | 69 | # fill the background and draw the square 70 | screen.fill((255, 255, 255)) 71 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 72 | screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]])) # ground 73 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([x[-1][0] + 3.0, x[-1][1]]), 74 | screen_projection([x[-1][0] - 3.0, x[-1][1]])) # ceil 75 | for eI in e: 76 | # ANCHOR: draw_tri 77 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 78 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[1]]), screen_projection(x[eI[2]])) 79 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[2]]), screen_projection(x[eI[0]])) 80 | # ANCHOR_END: draw_tri 81 | for xId in range(0, len(x) - 1): 82 | xI = x[xId] 83 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 84 | 85 | pygame.display.flip() # flip the display 86 | 87 | # step forward simulation and wait for screen refresh 88 | [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, ground_n, ground_o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit,DBC_stiff, h, 1e-2) 89 | time_step += 1 90 | pygame.time.wait(int(h * 1000)) 91 | square_mesh.write_to_file(time_step, x, e) 92 | 93 | pygame.quit() -------------------------------------------------------------------------------- /6_inv_free/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # ANCHOR: tri_vert_ind 13 | # connect the nodes with triangle elements 14 | e = [] 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg): 17 | # triangulate each cell following a symmetric pattern: 18 | if (i % 2)^(j % 2): 19 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 20 | e.append([(i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 21 | else: 22 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 23 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 24 | # ANCHOR_END: tri_vert_ind 25 | 26 | return [x, e] 27 | 28 | def write_to_file(frameNum, x, e): 29 | # Check if 'output' directory exists; if not, create it 30 | if not os.path.exists('output'): 31 | os.makedirs('output') 32 | 33 | # create obj file 34 | filename = f"output/{frameNum}.obj" 35 | with open(filename, 'w') as f: 36 | # write vertex coordinates 37 | for row in x: 38 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 39 | # write vertex indices for each triangle 40 | for row in e: 41 | #NOTE: vertex indices start from 1 in obj file format 42 | f.write(f"f {row[0] + 1} {row[1] + 1} {row[2] + 1}\n") -------------------------------------------------------------------------------- /6_inv_free/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import NeoHookeanEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | import FrictionEnergy 14 | import SpringEnergy 15 | 16 | 17 | 18 | 19 | def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, tol): 20 | x_tilde = x + v * h # implicit Euler predictive position 21 | x_n = copy.deepcopy(x) 22 | mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, contact_area, mu) # compute mu * lambda for each node using x^n 23 | DBC_target = [] # target position of each DBC in the current time step 24 | for i in range(0, len(DBC)): 25 | if (DBC_limit[i] - x_n[DBC[i]]).dot(DBC_v[i]) > 0: 26 | DBC_target.append(x_n[DBC[i]] + h * DBC_v[i]) 27 | else: 28 | DBC_target.append(x_n[DBC[i]]) 29 | 30 | # Newton loop 31 | iter = 0 32 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 33 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 34 | while (LA.norm(p, inf) / h > tol) | (sum(DBC_satisfied) != len(DBC)): # also check whether all DBCs are satisfied 35 | print('Iteration', iter, ':') 36 | print('residual =', LA.norm(p, inf) / h) 37 | 38 | if (LA.norm(p, inf) / h <= tol) & (sum(DBC_satisfied) != len(DBC)): 39 | # increase DBC stiffness and recompute energy value record 40 | DBC_stiff[0] *= 2 41 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 42 | 43 | # filter line search 44 | # ANCHOR: apply_filter 45 | alpha = min(BarrierEnergy.init_step_size(x, n, o, p), NeoHookeanEnergy.init_step_size(x, e, p)) # avoid interpenetration, tunneling, and inversion 46 | # ANCHOR_END: apply_filter 47 | while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x + alpha * p - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) > E_last: 48 | alpha /= 2 49 | print('step size =', alpha) 50 | 51 | x += alpha * p 52 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 53 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 54 | iter += 1 55 | 56 | v = (x - x_n) / h # implicit Euler velocity update 57 | return [x, v] 58 | 59 | def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 60 | return InertiaEnergy.val(x, x_tilde, m) + h * h * ( # implicit Euler 61 | NeoHookeanEnergy.val(x, e, vol, IB, mu_lame, lam) + 62 | GravityEnergy.val(x, m) + 63 | BarrierEnergy.val(x, n, o, contact_area) + 64 | FrictionEnergy.val(v, mu_lambda, h, n) 65 | ) + SpringEnergy.val(x, m, DBC, DBC_target, DBC_stiff) 66 | 67 | def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 68 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * ( # implicit Euler 69 | NeoHookeanEnergy.grad(x, e, vol, IB, mu_lame, lam) + 70 | GravityEnergy.grad(x, m) + 71 | BarrierEnergy.grad(x, n, o, contact_area) + 72 | FrictionEnergy.grad(v, mu_lambda, h, n) 73 | ) + SpringEnergy.grad(x, m, DBC, DBC_target, DBC_stiff) 74 | 75 | def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 76 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 77 | IJV_MS = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam) 78 | IJV_B = BarrierEnergy.hess(x, n, o, contact_area) 79 | IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n) 80 | IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff) 81 | IJV_MS[2] *= h * h # implicit Euler 82 | IJV_B[2] *= h * h # implicit Euler 83 | IJV_F[2] *= h * h # implicit Euler 84 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 85 | IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1) 86 | IJV_In_MS_B_F = np.append(IJV_In_MS_B, IJV_F, axis=1) 87 | IJV = np.append(IJV_In_MS_B_F, IJV_S, axis=1) 88 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 89 | return H 90 | 91 | def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h): 92 | projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h) 93 | reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1) 94 | # check whether each DBC is satisfied 95 | DBC_satisfied = [False] * len(x) 96 | for i in range(0, len(DBC)): 97 | if LA.norm(x[DBC[i]] - DBC_target[i]) / h < tol: 98 | DBC_satisfied[DBC[i]] = True 99 | # eliminate DOF if it's a satisfied DBC by modifying gradient and Hessian for DBC: 100 | for i, j in zip(*projected_hess.nonzero()): 101 | if (is_DBC[int(i / 2)] & DBC_satisfied[int(i / 2)]) | (is_DBC[int(j / 2)] & DBC_satisfied[int(j / 2)]): 102 | projected_hess[i, j] = (i == j) 103 | for i in range(0, len(x)): 104 | if is_DBC[i] & DBC_satisfied[i]: 105 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 106 | return [spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2), DBC_satisfied] -------------------------------------------------------------------------------- /6_inv_free/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | import math 4 | 5 | def make_PSD(hess): 6 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 7 | # set all negative Eigenvalues to 0 8 | for i in range(0, len(lam)): 9 | lam[i] = max(0, lam[i]) 10 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) 11 | 12 | # ANCHOR: find_positive_real_root 13 | def smallest_positive_real_root_quad(a, b, c, tol = 1e-6): 14 | # return negative value if no positive real root is found 15 | t = 0 16 | if abs(a) <= tol: 17 | if abs(b) <= tol: # f(x) = c > 0 for all x 18 | t = -1 19 | else: 20 | t = -c / b 21 | else: 22 | desc = b * b - 4 * a * c 23 | if desc > 0: 24 | t = (-b - math.sqrt(desc)) / (2 * a) 25 | if t < 0: 26 | t = (-b + math.sqrt(desc)) / (2 * a) 27 | else: # desv<0 ==> imag, f(x) > 0 for all x > 0 28 | t = -1 29 | return t 30 | # ANCHOR_END: find_positive_real_root -------------------------------------------------------------------------------- /7_self_contact/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | import distance.PointEdgeDistance as PE 5 | import distance.CCD as CCD 6 | 7 | import utils 8 | 9 | dhat = 0.01 10 | kappa = 1e5 11 | 12 | def val(x, n, o, bp, be, contact_area): 13 | sum = 0.0 14 | # floor: 15 | for i in range(0, len(x)): 16 | d = n.dot(x[i] - o) 17 | if d < dhat: 18 | s = d / dhat 19 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 20 | # ceil: 21 | n = np.array([0.0, -1.0]) 22 | for i in range(0, len(x) - 1): 23 | d = n.dot(x[i] - x[-1]) 24 | if d < dhat: 25 | s = d / dhat 26 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 27 | # ANCHOR: value 28 | # self-contact 29 | dhat_sqr = dhat * dhat 30 | for xI in bp: 31 | for eI in be: 32 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 33 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 34 | if d_sqr < dhat_sqr: 35 | s = d_sqr / dhat_sqr 36 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 37 | sum += 0.5 * contact_area[xI] * dhat * kappa / 8 * (s - 1) * math.log(s) 38 | # ANCHOR_END: value 39 | return sum 40 | 41 | def grad(x, n, o, bp, be, contact_area): 42 | g = np.array([[0.0, 0.0]] * len(x)) 43 | # floor: 44 | for i in range(0, len(x)): 45 | d = n.dot(x[i] - o) 46 | if d < dhat: 47 | s = d / dhat 48 | g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 49 | # ceil: 50 | n = np.array([0.0, -1.0]) 51 | for i in range(0, len(x) - 1): 52 | d = n.dot(x[i] - x[-1]) 53 | if d < dhat: 54 | s = d / dhat 55 | local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 56 | g[i] += local_grad 57 | g[-1] -= local_grad 58 | # ANCHOR: gradient 59 | # self-contact 60 | dhat_sqr = dhat * dhat 61 | for xI in bp: 62 | for eI in be: 63 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 64 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 65 | if d_sqr < dhat_sqr: 66 | s = d_sqr / dhat_sqr 67 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 68 | local_grad = 0.5 * contact_area[xI] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.grad(x[xI], x[eI[0]], x[eI[1]]) 69 | g[xI] += local_grad[0:2] 70 | g[eI[0]] += local_grad[2:4] 71 | g[eI[1]] += local_grad[4:6] 72 | # ANCHOR_END: gradient 73 | return g 74 | 75 | def hess(x, n, o, bp, be, contact_area): 76 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 77 | # floor: 78 | for i in range(0, len(x)): 79 | d = n.dot(x[i] - o) 80 | if d < dhat: 81 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 82 | for c in range(0, 2): 83 | for r in range(0, 2): 84 | IJV[0].append(i * 2 + r) 85 | IJV[1].append(i * 2 + c) 86 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 87 | # ceil: 88 | n = np.array([0.0, -1.0]) 89 | for i in range(0, len(x) - 1): 90 | d = n.dot(x[i] - x[-1]) 91 | if d < dhat: 92 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 93 | index = [i, len(x) - 1] 94 | for nI in range(0, 2): 95 | for nJ in range(0, 2): 96 | for c in range(0, 2): 97 | for r in range(0, 2): 98 | IJV[0].append(index[nI] * 2 + r) 99 | IJV[1].append(index[nJ] * 2 + c) 100 | IJV[2] = np.append(IJV[2], ((-1) ** (nI != nJ)) * local_hess[r, c]) 101 | # ANCHOR: Hessian 102 | # self-contact 103 | dhat_sqr = dhat * dhat 104 | for xI in bp: 105 | for eI in be: 106 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 107 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 108 | if d_sqr < dhat_sqr: 109 | d_sqr_grad = PE.grad(x[xI], x[eI[0]], x[eI[1]]) 110 | s = d_sqr / dhat_sqr 111 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 112 | local_hess = 0.5 * contact_area[xI] * dhat * utils.make_PSD(kappa / (8 * d_sqr * d_sqr * dhat_sqr) * (d_sqr + dhat_sqr) * np.outer(d_sqr_grad, d_sqr_grad) \ 113 | + (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.hess(x[xI], x[eI[0]], x[eI[1]])) 114 | index = [xI, eI[0], eI[1]] 115 | for nI in range(0, 3): 116 | for nJ in range(0, 3): 117 | for c in range(0, 2): 118 | for r in range(0, 2): 119 | IJV[0].append(index[nI] * 2 + r) 120 | IJV[1].append(index[nJ] * 2 + c) 121 | IJV[2] = np.append(IJV[2], local_hess[nI * 2 + r, nJ * 2 + c]) 122 | # ANCHOR_END: Hessian 123 | return IJV 124 | 125 | def init_step_size(x, n, o, bp, be, p): 126 | alpha = 1 127 | # floor: 128 | for i in range(0, len(x)): 129 | p_n = p[i].dot(n) 130 | if p_n < 0: 131 | alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n) 132 | # ceil: 133 | n = np.array([0.0, -1.0]) 134 | for i in range(0, len(x) - 1): 135 | p_n = (p[i] - p[-1]).dot(n) 136 | if p_n < 0: 137 | alpha = min(alpha, 0.9 * n.dot(x[i] - x[-1]) / -p_n) 138 | # ANCHOR: line_search_filtering 139 | # self-contact 140 | for xI in bp: 141 | for eI in be: 142 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 143 | if CCD.bbox_overlap(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha): 144 | toc = CCD.narrow_phase_CCD(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha) 145 | if alpha > toc: 146 | alpha = toc 147 | # ANCHOR_END: line_search_filtering 148 | return alpha 149 | 150 | def compute_mu_lambda(x, n, o, bp, be, contact_area, mu): 151 | # floor: 152 | mu_lambda = np.array([0.0] * len(x)) 153 | for i in range(0, len(x)): 154 | d = n.dot(x[i] - o) 155 | if d < dhat: 156 | s = d / dhat 157 | mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 158 | return mu_lambda -------------------------------------------------------------------------------- /7_self_contact/FrictionEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | epsv = 1e-3 5 | 6 | def f0(vbarnorm, epsv, hhat): 7 | if vbarnorm >= epsv: 8 | return vbarnorm * hhat 9 | else: 10 | vbarnormhhat = vbarnorm * hhat 11 | epsvhhat = epsv * hhat 12 | return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0 13 | 14 | def f1_div_vbarnorm(vbarnorm, epsv): 15 | if vbarnorm >= epsv: 16 | return 1.0 / vbarnorm 17 | else: 18 | return (-vbarnorm + 2.0 * epsv) / (epsv * epsv) 19 | 20 | def f_hess_term(vbarnorm, epsv): 21 | if vbarnorm >= epsv: 22 | return -1.0 / (vbarnorm * vbarnorm) 23 | else: 24 | return -1.0 / (epsv * epsv) 25 | 26 | def val(v, mu_lambda, hhat, n): 27 | sum = 0.0 28 | # floor: 29 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 30 | for i in range(0, len(v)): 31 | if mu_lambda[i] > 0: 32 | vbar = np.transpose(T).dot(v[i]) 33 | sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat) 34 | return sum 35 | 36 | def grad(v, mu_lambda, hhat, n): 37 | g = np.array([[0.0, 0.0]] * len(v)) 38 | # floor: 39 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 40 | for i in range(0, len(v)): 41 | if mu_lambda[i] > 0: 42 | vbar = np.transpose(T).dot(v[i]) 43 | g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 44 | return g 45 | 46 | def hess(v, mu_lambda, hhat, n): 47 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 48 | # floor: 49 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 50 | for i in range(0, len(v)): 51 | if mu_lambda[i] > 0: 52 | vbar = np.transpose(T).dot(v[i]) 53 | vbarnorm = np.linalg.norm(vbar) 54 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 55 | if vbarnorm != 0: 56 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 57 | local_hess = mu_lambda[i] * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 58 | for c in range(0, 2): 59 | for r in range(0, 2): 60 | IJV[0].append(i * 2 + r) 61 | IJV[1].append(i * 2 + c) 62 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 63 | return IJV -------------------------------------------------------------------------------- /7_self_contact/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /7_self_contact/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /7_self_contact/NeoHookeanEnergy.py: -------------------------------------------------------------------------------- 1 | import utils 2 | import numpy as np 3 | import math 4 | 5 | def polar_svd(F): 6 | [U, s, VT] = np.linalg.svd(F) 7 | if np.linalg.det(U) < 0: 8 | U[:, 1] = -U[:, 1] 9 | s[1] = -s[1] 10 | if np.linalg.det(VT) < 0: 11 | VT[1, :] = -VT[1, :] 12 | s[1] = -s[1] 13 | return [U, s, VT] 14 | 15 | def dPsi_div_dsigma(s, mu, lam): 16 | ln_sigma_prod = math.log(s[0] * s[1]) 17 | inv0 = 1.0 / s[0] 18 | dPsi_dsigma_0 = mu * (s[0] - inv0) + lam * inv0 * ln_sigma_prod 19 | inv1 = 1.0 / s[1] 20 | dPsi_dsigma_1 = mu * (s[1] - inv1) + lam * inv1 * ln_sigma_prod 21 | return [dPsi_dsigma_0, dPsi_dsigma_1] 22 | 23 | def d2Psi_div_dsigma2(s, mu, lam): 24 | ln_sigma_prod = math.log(s[0] * s[1]) 25 | inv2_0 = 1 / (s[0] * s[0]) 26 | d2Psi_dsigma2_00 = mu * (1 + inv2_0) - lam * inv2_0 * (ln_sigma_prod - 1) 27 | inv2_1 = 1 / (s[1] * s[1]) 28 | d2Psi_dsigma2_11 = mu * (1 + inv2_1) - lam * inv2_1 * (ln_sigma_prod - 1) 29 | d2Psi_dsigma2_01 = lam / (s[0] * s[1]) 30 | return [[d2Psi_dsigma2_00, d2Psi_dsigma2_01], [d2Psi_dsigma2_01, d2Psi_dsigma2_11]] 31 | 32 | def B_left_coef(s, mu, lam): 33 | sigma_prod = s[0] * s[1] 34 | return (mu + (mu - lam * math.log(sigma_prod)) / sigma_prod) / 2 35 | 36 | def Psi(F, mu, lam): 37 | J = np.linalg.det(F) 38 | lnJ = math.log(J) 39 | return mu / 2 * (np.trace(np.transpose(F).dot(F)) - 2) - mu * lnJ + lam / 2 * lnJ * lnJ 40 | 41 | def dPsi_div_dF(F, mu, lam): 42 | FinvT = np.transpose(np.linalg.inv(F)) 43 | return mu * (F - FinvT) + lam * math.log(np.linalg.det(F)) * FinvT 44 | 45 | def d2Psi_div_dF2(F, mu, lam): 46 | [U, sigma, VT] = polar_svd(F) 47 | 48 | Psi_sigma_sigma = utils.make_PSD(d2Psi_div_dsigma2(sigma, mu, lam)) 49 | 50 | B_left = B_left_coef(sigma, mu, lam) 51 | Psi_sigma = dPsi_div_dsigma(sigma, mu, lam) 52 | B_right = (Psi_sigma[0] + Psi_sigma[1]) / (2 * max(sigma[0] + sigma[1], 1e-6)) 53 | B = utils.make_PSD([[B_left + B_right, B_left - B_right], [B_left - B_right, B_left + B_right]]) 54 | 55 | M = np.array([[0, 0, 0, 0]] * 4) 56 | M[0, 0] = Psi_sigma_sigma[0, 0] 57 | M[0, 3] = Psi_sigma_sigma[0, 1] 58 | M[1, 1] = B[0, 0] 59 | M[1, 2] = B[0, 1] 60 | M[2, 1] = B[1, 0] 61 | M[2, 2] = B[1, 1] 62 | M[3, 0] = Psi_sigma_sigma[1, 0] 63 | M[3, 3] = Psi_sigma_sigma[1, 1] 64 | 65 | dP_div_dF = np.array([[0, 0, 0, 0]] * 4) 66 | for j in range(0, 2): 67 | for i in range(0, 2): 68 | ij = j * 2 + i 69 | for s in range(0, 2): 70 | for r in range(0, 2): 71 | rs = s * 2 + r 72 | dP_div_dF[ij, rs] = M[0, 0] * U[i, 0] * VT[0, j] * U[r, 0] * VT[0, s] \ 73 | + M[0, 3] * U[i, 0] * VT[0, j] * U[r, 1] * VT[1, s] \ 74 | + M[1, 1] * U[i, 1] * VT[0, j] * U[r, 1] * VT[0, s] \ 75 | + M[1, 2] * U[i, 1] * VT[0, j] * U[r, 0] * VT[1, s] \ 76 | + M[2, 1] * U[i, 0] * VT[1, j] * U[r, 1] * VT[0, s] \ 77 | + M[2, 2] * U[i, 0] * VT[1, j] * U[r, 0] * VT[1, s] \ 78 | + M[3, 0] * U[i, 1] * VT[1, j] * U[r, 0] * VT[0, s] \ 79 | + M[3, 3] * U[i, 1] * VT[1, j] * U[r, 1] * VT[1, s] 80 | return dP_div_dF 81 | 82 | def deformation_grad(x, elemVInd, IB): 83 | F = [x[elemVInd[1]] - x[elemVInd[0]], x[elemVInd[2]] - x[elemVInd[0]]] 84 | return np.transpose(F).dot(IB) 85 | 86 | def dPsi_div_dx(P, IB): # applying chain-rule, dPsi_div_dx = dPsi_div_dF * dF_div_dx 87 | dPsi_dx_2 = P[0, 0] * IB[0, 0] + P[0, 1] * IB[0, 1] 88 | dPsi_dx_3 = P[1, 0] * IB[0, 0] + P[1, 1] * IB[0, 1] 89 | dPsi_dx_4 = P[0, 0] * IB[1, 0] + P[0, 1] * IB[1, 1] 90 | dPsi_dx_5 = P[1, 0] * IB[1, 0] + P[1, 1] * IB[1, 1] 91 | return [np.array([-dPsi_dx_2 - dPsi_dx_4, -dPsi_dx_3 - dPsi_dx_5]), np.array([dPsi_dx_2, dPsi_dx_3]), np.array([dPsi_dx_4, dPsi_dx_5])] 92 | 93 | def d2Psi_div_dx2(dP_div_dF, IB): # applying chain-rule, d2Psi_div_dx2 = dF_div_dx^T * d2Psi_div_dF2 * dF_div_dx (note that d2F_div_dx2 = 0) 94 | intermediate = np.array([[0.0, 0.0, 0.0, 0.0]] * 6) 95 | for colI in range(0, 4): 96 | _000 = dP_div_dF[0, colI] * IB[0, 0] 97 | _010 = dP_div_dF[0, colI] * IB[1, 0] 98 | _101 = dP_div_dF[2, colI] * IB[0, 1] 99 | _111 = dP_div_dF[2, colI] * IB[1, 1] 100 | _200 = dP_div_dF[1, colI] * IB[0, 0] 101 | _210 = dP_div_dF[1, colI] * IB[1, 0] 102 | _301 = dP_div_dF[3, colI] * IB[0, 1] 103 | _311 = dP_div_dF[3, colI] * IB[1, 1] 104 | intermediate[2, colI] = _000 + _101 105 | intermediate[3, colI] = _200 + _301 106 | intermediate[4, colI] = _010 + _111 107 | intermediate[5, colI] = _210 + _311 108 | intermediate[0, colI] = -intermediate[2, colI] - intermediate[4, colI] 109 | intermediate[1, colI] = -intermediate[3, colI] - intermediate[5, colI] 110 | result = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * 6) 111 | for colI in range(0, 6): 112 | _000 = intermediate[colI, 0] * IB[0, 0] 113 | _010 = intermediate[colI, 0] * IB[1, 0] 114 | _101 = intermediate[colI, 2] * IB[0, 1] 115 | _111 = intermediate[colI, 2] * IB[1, 1] 116 | _200 = intermediate[colI, 1] * IB[0, 0] 117 | _210 = intermediate[colI, 1] * IB[1, 0] 118 | _301 = intermediate[colI, 3] * IB[0, 1] 119 | _311 = intermediate[colI, 3] * IB[1, 1] 120 | result[2, colI] = _000 + _101 121 | result[3, colI] = _200 + _301 122 | result[4, colI] = _010 + _111 123 | result[5, colI] = _210 + _311 124 | result[0, colI] = -_000 - _101 - _010 - _111 125 | result[1, colI] = -_200 - _301 - _210 - _311 126 | return result 127 | 128 | def val(x, e, vol, IB, mu, lam): 129 | sum = 0.0 130 | for i in range(0, len(e)): 131 | F = deformation_grad(x, e[i], IB[i]) 132 | sum += vol[i] * Psi(F, mu[i], lam[i]) 133 | return sum 134 | 135 | def grad(x, e, vol, IB, mu, lam): 136 | g = np.array([[0.0, 0.0]] * len(x)) 137 | for i in range(0, len(e)): 138 | F = deformation_grad(x, e[i], IB[i]) 139 | P = vol[i] * dPsi_div_dF(F, mu[i], lam[i]) 140 | g_local = dPsi_div_dx(P, IB[i]) 141 | for j in range(0, 3): 142 | g[e[i][j]] += g_local[j] 143 | return g 144 | 145 | def hess(x, e, vol, IB, mu, lam): 146 | IJV = [[0] * (len(e) * 36), [0] * (len(e) * 36), np.array([0.0] * (len(e) * 36))] 147 | for i in range(0, len(e)): 148 | F = deformation_grad(x, e[i], IB[i]) 149 | dP_div_dF = vol[i] * d2Psi_div_dF2(F, mu[i], lam[i]) 150 | local_hess = d2Psi_div_dx2(dP_div_dF, IB[i]) 151 | for xI in range(0, 3): 152 | for xJ in range(0, 3): 153 | for dI in range(0, 2): 154 | for dJ in range(0, 2): 155 | ind = i * 36 + (xI * 3 + xJ) * 4 + dI * 2 + dJ 156 | IJV[0][ind] = e[i][xI] * 2 + dI 157 | IJV[1][ind] = e[i][xJ] * 2 + dJ 158 | IJV[2][ind] = local_hess[xI * 2 + dI, xJ * 2 + dJ] 159 | return IJV 160 | 161 | def init_step_size(x, e, p): 162 | alpha = 1 163 | for i in range(0, len(e)): 164 | x21 = x[e[i][1]] - x[e[i][0]] 165 | x31 = x[e[i][2]] - x[e[i][0]] 166 | p21 = p[e[i][1]] - p[e[i][0]] 167 | p31 = p[e[i][2]] - p[e[i][0]] 168 | detT = np.linalg.det(np.transpose([x21, x31])) 169 | a = np.linalg.det(np.transpose([p21, p31])) / detT 170 | b = (np.linalg.det(np.transpose([x21, p31])) + np.linalg.det(np.transpose([p21, x31]))) / detT 171 | c = 0.9 # solve for alpha that first brings the new volume to 0.1x the old volume for slackness 172 | critical_alpha = utils.smallest_positive_real_root_quad(a, b, c) 173 | if critical_alpha > 0: 174 | alpha = min(alpha, critical_alpha) 175 | return alpha -------------------------------------------------------------------------------- /7_self_contact/SpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, m, DBC, DBC_target, k): 4 | sum = 0.0 5 | for i in range(0, len(DBC)): 6 | diff = x[DBC[i]] - DBC_target[i] 7 | sum += 0.5 * k * m[DBC[i]] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, m, DBC, DBC_target, k): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(DBC)): 13 | g[DBC[i]] = k * m[DBC[i]] * (x[DBC[i]] - DBC_target[i]) 14 | return g 15 | 16 | def hess(x, m, DBC, DBC_target, k): 17 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 18 | for i in range(0, len(DBC)): 19 | for d in range(0, 2): 20 | IJV[0].append(DBC[i] * 2 + d) 21 | IJV[1].append(DBC[i] * 2 + d) 22 | IJV[2] = np.append(IJV[2], k * m[DBC[i]]) 23 | return IJV -------------------------------------------------------------------------------- /7_self_contact/distance/CCD.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: broad_phase 2 | from copy import deepcopy 3 | import numpy as np 4 | import math 5 | 6 | import distance.PointEdgeDistance as PE 7 | 8 | # check whether the bounding box of the trajectory of the point and the edge overlap 9 | def bbox_overlap(p, e0, e1, dp, de0, de1, toc_upperbound): 10 | max_p = np.maximum(p, p + toc_upperbound * dp) # point trajectory bbox top-right 11 | min_p = np.minimum(p, p + toc_upperbound * dp) # point trajectory bbox bottom-left 12 | max_e = np.maximum(np.maximum(e0, e0 + toc_upperbound * de0), np.maximum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox top-right 13 | min_e = np.minimum(np.minimum(e0, e0 + toc_upperbound * de0), np.minimum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox bottom-left 14 | if np.any(np.greater(min_p, max_e)) or np.any(np.greater(min_e, max_p)): 15 | return False 16 | else: 17 | return True 18 | # ANCHOR_END: broad_phase 19 | 20 | # ANCHOR: accd 21 | # compute the first "time" of contact, or toc, 22 | # return the computed toc only if it is smaller than the previously computed toc_upperbound 23 | def narrow_phase_CCD(_p, _e0, _e1, _dp, _de0, _de1, toc_upperbound): 24 | p = deepcopy(_p) 25 | e0 = deepcopy(_e0) 26 | e1 = deepcopy(_e1) 27 | dp = deepcopy(_dp) 28 | de0 = deepcopy(_de0) 29 | de1 = deepcopy(_de1) 30 | 31 | # use relative displacement for faster convergence 32 | mov = (dp + de0 + de1) / 3 33 | de0 -= mov 34 | de1 -= mov 35 | dp -= mov 36 | maxDispMag = np.linalg.norm(dp) + math.sqrt(max(np.dot(de0, de0), np.dot(de1, de1))) 37 | if maxDispMag == 0: 38 | return toc_upperbound 39 | 40 | eta = 0.1 # calculate the toc that first brings the distance to 0.1x the current distance 41 | dist2_cur = PE.val(p, e0, e1) 42 | dist_cur = math.sqrt(dist2_cur) 43 | gap = eta * dist_cur 44 | # iteratively move the point and edge towards each other and 45 | # grow the toc estimate without numerical errors 46 | toc = 0 47 | while True: 48 | tocLowerBound = (1 - eta) * dist_cur / maxDispMag 49 | 50 | p += tocLowerBound * dp 51 | e0 += tocLowerBound * de0 52 | e1 += tocLowerBound * de1 53 | dist2_cur = PE.val(p, e0, e1) 54 | dist_cur = math.sqrt(dist2_cur) 55 | if toc != 0 and dist_cur < gap: 56 | break 57 | 58 | toc += tocLowerBound 59 | if toc > toc_upperbound: 60 | return toc_upperbound 61 | 62 | return toc 63 | # ANCHOR_END: accd -------------------------------------------------------------------------------- /7_self_contact/distance/PointEdgeDistance.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: PE_val_grad 2 | import numpy as np 3 | 4 | import distance.PointPointDistance as PP 5 | import distance.PointLineDistance as PL 6 | 7 | def val(p, e0, e1): 8 | e = e1 - e0 9 | ratio = np.dot(e, p - e0) / np.dot(e, e) 10 | if ratio < 0: # point(p)-point(e0) expression 11 | return PP.val(p, e0) 12 | elif ratio > 1: # point(p)-point(e1) expression 13 | return PP.val(p, e1) 14 | else: # point(p)-line(e0e1) expression 15 | return PL.val(p, e0, e1) 16 | 17 | def grad(p, e0, e1): 18 | e = e1 - e0 19 | ratio = np.dot(e, p - e0) / np.dot(e, e) 20 | if ratio < 0: # point(p)-point(e0) expression 21 | g_PP = PP.grad(p, e0) 22 | return np.reshape([g_PP[0:2], g_PP[2:4], np.array([0.0, 0.0])], (1, 6))[0] 23 | elif ratio > 1: # point(p)-point(e1) expression 24 | g_PP = PP.grad(p, e1) 25 | return np.reshape([g_PP[0:2], np.array([0.0, 0.0]), g_PP[2:4]], (1, 6))[0] 26 | else: # point(p)-line(e0e1) expression 27 | return PL.grad(p, e0, e1) 28 | # ANCHOR_END: PE_val_grad 29 | 30 | def hess(p, e0, e1): 31 | e = e1 - e0 32 | ratio = np.dot(e, p - e0) / np.dot(e, e) 33 | if ratio < 0: # point(p)-point(e0) expression 34 | H_PP = PP.hess(p, e0) 35 | return np.array([np.reshape([H_PP[0, 0:2], H_PP[0, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 36 | np.reshape([H_PP[1, 0:2], H_PP[1, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 37 | np.reshape([H_PP[2, 0:2], H_PP[2, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 38 | np.reshape([H_PP[3, 0:2], H_PP[3, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 39 | np.array([0.0] * 6), \ 40 | np.array([0.0] * 6)]) 41 | elif ratio > 1: # point(p)-point(e1) expression 42 | H_PP = PP.hess(p, e1) 43 | return np.array([np.reshape([H_PP[0, 0:2], np.array([0.0, 0.0]), H_PP[0, 2:4]], (1, 6))[0], \ 44 | np.reshape([H_PP[1, 0:2], np.array([0.0, 0.0]), H_PP[1, 2:4]], (1, 6))[0], \ 45 | np.array([0.0] * 6), \ 46 | np.array([0.0] * 6), \ 47 | np.reshape([H_PP[2, 0:2], np.array([0.0, 0.0]), H_PP[2, 2:4]], (1, 6))[0], \ 48 | np.reshape([H_PP[3, 0:2], np.array([0.0, 0.0]), H_PP[3, 2:4]], (1, 6))[0]]) 49 | else: # point(p)-line(e0e1) expression 50 | return PL.hess(p, e0, e1) -------------------------------------------------------------------------------- /7_self_contact/distance/PointLineDistance.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: PL_val_grad 2 | import numpy as np 3 | 4 | def val(p, e0, e1): 5 | e = e1 - e0 6 | numerator = e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0] 7 | return numerator * numerator / np.dot(e, e) 8 | 9 | def grad(p, e0, e1): 10 | g = np.array([0.0] * 6) 11 | t13 = -e1[0] + e0[0] 12 | t14 = -e1[1] + e0[1] 13 | t23 = 1.0 / (t13 * t13 + t14 * t14) 14 | t25 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t14 * p[0]) + -(t13 * p[1]) 15 | t24 = t23 * t23 16 | t26 = t25 * t25 17 | t27 = (e0[0] * 2.0 + -(e1[0] * 2.0)) * t24 * t26 18 | t26 *= (e0[1] * 2.0 + -(e1[1] * 2.0)) * t24 19 | g[0] = t14 * t23 * t25 * 2.0 20 | g[1] = t13 * t23 * t25 * -2.0 21 | t24 = t23 * t25 22 | g[2] = -t27 - t24 * (-e1[1] + p[1]) * 2.0 23 | g[3] = -t26 + t24 * (-e1[0] + p[0]) * 2.0 24 | g[4] = t27 + t24 * (p[1] - e0[1]) * 2.0 25 | g[5] = t26 - t24 * (p[0] - e0[0]) * 2.0 26 | return g 27 | # ANCHOR_END: PL_val_grad 28 | 29 | def hess(p, e0, e1): 30 | H = np.array([0.0] * 36) 31 | t15 = -e0[0] + p[0] 32 | t16 = -e0[1] + p[1] 33 | t17 = -e1[0] + p[0] 34 | t18 = -e1[1] + p[1] 35 | t19 = -e1[0] + e0[0] 36 | t20 = -e1[1] + e0[1] 37 | t21 = e0[0] * 2.0 + -(e1[0] * 2.0) 38 | t22 = e0[1] * 2.0 + -(e1[1] * 2.0) 39 | t23 = t19 * t19 40 | t24 = t20 * t20 41 | t31 = 1.0 / (t23 + t24) 42 | t34 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t20 * p[0]) + -(t19 * p[1]) 43 | t32 = t31 * t31 44 | t33 = t32 * t31 45 | t35 = t34 * t34 46 | t60 = t31 * t34 * 2.0 47 | t59 = -(t19 * t20 * t31 * 2.0) 48 | t62 = t32 * t35 * 2.0 49 | t64 = t21 * t21 * t33 * t35 * 2.0 50 | t65 = t22 * t22 * t33 * t35 * 2.0 51 | t68 = t15 * t21 * t32 * t34 * 2.0 52 | t71 = t16 * t22 * t32 * t34 * 2.0 53 | t72 = t17 * t21 * t32 * t34 * 2.0 54 | t75 = t18 * t22 * t32 * t34 * 2.0 55 | t76 = t19 * t21 * t32 * t34 * 2.0 56 | t77 = t20 * t21 * t32 * t34 * 2.0 57 | t78 = t19 * t22 * t32 * t34 * 2.0 58 | t79 = t20 * t22 * t32 * t34 * 2.0 59 | t90 = t21 * t22 * t33 * t35 * 2.0 60 | t92 = t16 * t20 * t31 * 2.0 + t77 61 | t94 = -(t17 * t19 * t31 * 2.0) + t78 62 | t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76 63 | t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79 64 | t93 = t15 * t19 * t31 * 2.0 + -t78 65 | t35 = -(t18 * t20 * t31 * 2.0) + -t77 66 | t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79 67 | t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76 68 | t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90 69 | t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90 70 | t102_tmp = t17 * t22 * t32 * t34 71 | t76 = t15 * t22 * t32 * t34 72 | t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0) + t102_tmp * 2.0 73 | t33 = t18 * t21 * t32 * t34 74 | t20 = t16 * t21 * t32 * t34 75 | t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0)) + -(t33 * 2.0) 76 | t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90 77 | t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90 78 | H[0] = t24 * t31 * 2.0 79 | H[1] = t59 80 | H[2] = t35 81 | H[3] = t97 82 | H[4] = t92 83 | H[5] = t99 84 | H[6] = t59 85 | H[7] = t23 * t31 * 2.0 86 | H[8] = t96 87 | H[9] = t94 88 | H[10] = t98 89 | H[11] = t93 90 | H[12] = t35 91 | H[13] = t96 92 | t35 = -t62 + t64 93 | H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0 94 | H[15] = t19 95 | H[16] = t79 96 | H[17] = t77 97 | H[18] = t97 98 | H[19] = t94 99 | H[20] = t19 100 | t33 = -t62 + t65 101 | H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0 102 | H[22] = t78 103 | H[23] = t22 104 | H[24] = t92 105 | H[25] = t98 106 | H[26] = t79 107 | H[27] = t78 108 | H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0 109 | H[29] = t100 110 | H[30] = t99 111 | H[31] = t93 112 | H[32] = t77 113 | H[33] = t22 114 | H[34] = t100 115 | H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0 116 | return np.reshape(H, (6, 6)) -------------------------------------------------------------------------------- /7_self_contact/distance/PointPointDistance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(p0, p1): 4 | e = p0 - p1 5 | return np.dot(e, e) 6 | 7 | def grad(p0, p1): 8 | e = p0 - p1 9 | return np.reshape([2 * e, -2 * e], (1, 4))[0] 10 | 11 | def hess(p0, p1): 12 | H = np.array([[0.0] * 4] * 4) 13 | H[0, 0] = H[1, 1] = H[2, 2] = H[3, 3] = 2 14 | H[0, 2] = H[1, 3] = H[2, 0] = H[3, 1] = -2 15 | return H -------------------------------------------------------------------------------- /7_self_contact/readme.md: -------------------------------------------------------------------------------- 1 | # Inversion-free Hyperelastic Solids Simulation 2 | 3 | Two squares falling onto the ground under gravity, contacting with each other, and then compressed by a ceiling is simulated with an inversion-free hyperelastic potential and IPC with implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /7_self_contact/simulator.py: -------------------------------------------------------------------------------- 1 | # FEM Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # ANCHOR: sim_setup 11 | # simulation setup 12 | side_len = 0.45 13 | rho = 1000 # density of square 14 | E = 1e5 # Young's modulus 15 | nu = 0.4 # Poisson's ratio 16 | n_seg = 2 # num of segments per side of the square 17 | h = 0.01 # time step size in s 18 | DBC = [(n_seg + 1) * (n_seg + 1) * 2] # dirichlet node index 19 | DBC_v = [np.array([0.0, -0.5])] # dirichlet node velocity 20 | DBC_limit = [np.array([0.0, -0.7])] # dirichlet node limit position 21 | ground_n = np.array([0.0, 1.0]) # normal of the slope 22 | ground_n /= np.linalg.norm(ground_n) # normalize ground normal vector just in case 23 | ground_o = np.array([0.0, -1.0]) # a point on the slope 24 | mu = 0.4 # friction coefficient of the slope 25 | 26 | # initialize simulation 27 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and triangle node indices of the top square 28 | e = np.append(e, np.array(e) + [len(x)] * 3, axis=0) # add triangle node indices of the bottom square 29 | x = np.append(x, x + [side_len * 0.1, -side_len * 1.1], axis=0) # add node positions of the bottom square 30 | # ANCHOR_END: sim_setup 31 | [bp, be] = square_mesh.find_boundary(e) # find boundary points and edges for self-contact 32 | x = np.append(x, [[0.0, side_len * 0.6]], axis=0) # ceil origin (with normal [0.0, -1.0]) 33 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 34 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 35 | # rest shape basis, volume, and lame parameters 36 | vol = [0.0] * len(e) 37 | IB = [np.array([[0.0, 0.0]] * 2)] * len(e) 38 | for i in range(0, len(e)): 39 | TB = [x[e[i][1]] - x[e[i][0]], x[e[i][2]] - x[e[i][0]]] 40 | vol[i] = np.linalg.det(np.transpose(TB)) / 2 41 | IB[i] = np.linalg.inv(np.transpose(TB)) 42 | mu_lame = [0.5 * E / (1 + nu)] * len(e) 43 | lam = [E * nu / ((1 + nu) * (1 - 2 * nu))] * len(e) 44 | # identify whether a node is Dirichlet 45 | is_DBC = [False] * len(x) 46 | for i in DBC: 47 | is_DBC[i] = True 48 | DBC_stiff = [1000] # DBC stiffness, adjusted and warm-started across time steps 49 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 50 | 51 | # simulation with visualization 52 | resolution = np.array([900, 900]) 53 | offset = resolution / 2 54 | scale = 200 55 | def screen_projection(x): 56 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 57 | 58 | time_step = 0 59 | square_mesh.write_to_file(time_step, x, e) 60 | screen = pygame.display.set_mode(resolution) 61 | running = True 62 | while running: 63 | # run until the user asks to quit 64 | for event in pygame.event.get(): 65 | if event.type == pygame.QUIT: 66 | running = False 67 | 68 | print('### Time step', time_step, '###') 69 | 70 | # fill the background and draw the square 71 | screen.fill((255, 255, 255)) 72 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 73 | screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]])) # ground 74 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([x[-1][0] + 3.0, x[-1][1]]), 75 | screen_projection([x[-1][0] - 3.0, x[-1][1]])) # ceil 76 | for eI in e: 77 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 78 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[1]]), screen_projection(x[eI[2]])) 79 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[2]]), screen_projection(x[eI[0]])) 80 | for xId in range(0, len(x) - 1): 81 | xI = x[xId] 82 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 83 | 84 | pygame.display.flip() # flip the display 85 | 86 | # step forward simulation and wait for screen refresh 87 | [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, ground_n, ground_o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, 1e-2) 88 | time_step += 1 89 | pygame.time.wait(int(h * 1000)) 90 | square_mesh.write_to_file(time_step, x, e) 91 | 92 | pygame.quit() -------------------------------------------------------------------------------- /7_self_contact/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with triangle elements 13 | e = [] 14 | for i in range(0, n_seg): 15 | for j in range(0, n_seg): 16 | # triangulate each cell following a symmetric pattern: 17 | if (i % 2)^(j % 2): 18 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 19 | e.append([(i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 20 | else: 21 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 22 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 23 | 24 | return [x, e] 25 | 26 | # ANCHOR: find_boundary 27 | def find_boundary(e): 28 | # index all half-edges for fast query 29 | edge_set = set() 30 | for i in range(0, len(e)): 31 | for j in range(0, 3): 32 | edge_set.add((e[i][j], e[i][(j + 1) % 3])) 33 | 34 | # find boundary points and edges 35 | bp_set = set() 36 | be = [] 37 | for eI in edge_set: 38 | if (eI[1], eI[0]) not in edge_set: 39 | # if the inverse edge of a half-edge does not exist, 40 | # then it is a boundary edge 41 | be.append([eI[0], eI[1]]) 42 | bp_set.add(eI[0]) 43 | bp_set.add(eI[1]) 44 | return [list(bp_set), be] 45 | # ANCHOR_END: find_boundary 46 | 47 | def write_to_file(frameNum, x, e): 48 | # Check if 'output' directory exists; if not, create it 49 | if not os.path.exists('output'): 50 | os.makedirs('output') 51 | 52 | # create obj file 53 | filename = f"output/{frameNum}.obj" 54 | with open(filename, 'w') as f: 55 | # write vertex coordinates 56 | for row in x: 57 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 58 | # write vertex indices for each triangle 59 | for row in e: 60 | #NOTE: vertex indices start from 1 in obj file format 61 | f.write(f"f {row[0] + 1} {row[1] + 1} {row[2] + 1}\n") -------------------------------------------------------------------------------- /7_self_contact/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import NeoHookeanEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | import FrictionEnergy 14 | import SpringEnergy 15 | 16 | def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, tol): 17 | x_tilde = x + v * h # implicit Euler predictive position 18 | x_n = copy.deepcopy(x) 19 | mu_lambda = BarrierEnergy.compute_mu_lambda(x, n, o, bp, be, contact_area, mu) # compute mu * lambda for each node using x^n 20 | DBC_target = [] # target position of each DBC in the current time step 21 | for i in range(0, len(DBC)): 22 | if (DBC_limit[i] - x_n[DBC[i]]).dot(DBC_v[i]) > 0: 23 | DBC_target.append(x_n[DBC[i]] + h * DBC_v[i]) 24 | else: 25 | DBC_target.append(x_n[DBC[i]]) 26 | 27 | # Newton loop 28 | iter = 0 29 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 30 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 31 | while (LA.norm(p, inf) / h > tol) | (sum(DBC_satisfied) != len(DBC)): # also check whether all DBCs are satisfied 32 | print('Iteration', iter, ':') 33 | print('residual =', LA.norm(p, inf) / h) 34 | 35 | if (LA.norm(p, inf) / h <= tol) & (sum(DBC_satisfied) != len(DBC)): 36 | # increase DBC stiffness and recompute energy value record 37 | DBC_stiff[0] *= 2 38 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 39 | 40 | # filter line search 41 | alpha = min(BarrierEnergy.init_step_size(x, n, o, bp, be, p), NeoHookeanEnergy.init_step_size(x, e, p)) # avoid interpenetration, tunneling, and inversion 42 | while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x + alpha * p - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) > E_last: 43 | alpha /= 2 44 | print('step size =', alpha) 45 | 46 | x += alpha * p 47 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, DBC, DBC_target, DBC_stiff[0], h) 48 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 49 | iter += 1 50 | 51 | v = (x - x_n) / h # implicit Euler velocity update 52 | return [x, v] 53 | 54 | def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 55 | return InertiaEnergy.val(x, x_tilde, m) + h * h * ( # implicit Euler 56 | NeoHookeanEnergy.val(x, e, vol, IB, mu_lame, lam) + 57 | GravityEnergy.val(x, m) + 58 | BarrierEnergy.val(x, n, o, bp, be, contact_area) + 59 | FrictionEnergy.val(v, mu_lambda, h, n) 60 | ) + SpringEnergy.val(x, m, DBC, DBC_target, DBC_stiff) 61 | 62 | def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 63 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * ( # implicit Euler 64 | NeoHookeanEnergy.grad(x, e, vol, IB, mu_lame, lam) + 65 | GravityEnergy.grad(x, m) + 66 | BarrierEnergy.grad(x, n, o, bp, be, contact_area) + 67 | FrictionEnergy.grad(v, mu_lambda, h, n) 68 | ) + SpringEnergy.grad(x, m, DBC, DBC_target, DBC_stiff) 69 | 70 | def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h): 71 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 72 | IJV_MS = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam) 73 | IJV_B = BarrierEnergy.hess(x, n, o, bp, be, contact_area) 74 | IJV_F = FrictionEnergy.hess(v, mu_lambda, h, n) 75 | IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff) 76 | IJV_MS[2] *= h * h # implicit Euler 77 | IJV_B[2] *= h * h # implicit Euler 78 | IJV_F[2] *= h * h # implicit Euler 79 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 80 | IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1) 81 | IJV_In_MS_B_F = np.append(IJV_In_MS_B, IJV_F, axis=1) 82 | IJV = np.append(IJV_In_MS_B_F, IJV_S, axis=1) 83 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 84 | return H 85 | 86 | def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, is_DBC, DBC, DBC_target, DBC_stiff, tol, h): 87 | projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h) 88 | reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1) 89 | # check whether each DBC is satisfied 90 | DBC_satisfied = [False] * len(x) 91 | for i in range(0, len(DBC)): 92 | if LA.norm(x[DBC[i]] - DBC_target[i]) / h < tol: 93 | DBC_satisfied[DBC[i]] = True 94 | # eliminate DOF if it's a satisfied DBC by modifying gradient and Hessian for DBC: 95 | for i, j in zip(*projected_hess.nonzero()): 96 | if (is_DBC[int(i / 2)] & DBC_satisfied[int(i / 2)]) | (is_DBC[int(j / 2)] & DBC_satisfied[int(j / 2)]): 97 | projected_hess[i, j] = (i == j) 98 | for i in range(0, len(x)): 99 | if is_DBC[i] & DBC_satisfied[i]: 100 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 101 | return [spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2), DBC_satisfied] -------------------------------------------------------------------------------- /7_self_contact/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | import math 4 | 5 | def make_PSD(hess): 6 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 7 | # set all negative Eigenvalues to 0 8 | for i in range(0, len(lam)): 9 | lam[i] = max(0, lam[i]) 10 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) 11 | 12 | def smallest_positive_real_root_quad(a, b, c, tol = 1e-6): 13 | # return negative value if no positive real root is found 14 | t = 0 15 | if abs(a) <= tol: 16 | if abs(b) <= tol: # f(x) = c > 0 for all x 17 | t = -1 18 | else: 19 | t = -c / b 20 | else: 21 | desc = b * b - 4 * a * c 22 | if desc > 0: 23 | t = (-b - math.sqrt(desc)) / (2 * a) 24 | if t < 0: 25 | t = (-b + math.sqrt(desc)) / (2 * a) 26 | else: # desv<0 ==> imag, f(x) > 0 for all x > 0 27 | t = -1 28 | return t -------------------------------------------------------------------------------- /8_self_friction/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | import distance.PointEdgeDistance as PE 5 | import distance.CCD as CCD 6 | 7 | import utils 8 | 9 | dhat = 0.01 10 | kappa = 1e5 11 | 12 | def val(x, n, o, bp, be, contact_area): 13 | sum = 0.0 14 | # floor: 15 | for i in range(0, len(x)): 16 | d = n.dot(x[i] - o) 17 | if d < dhat: 18 | s = d / dhat 19 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 20 | # ceil: 21 | n = np.array([0.0, -1.0]) 22 | for i in range(0, len(x) - 1): 23 | d = n.dot(x[i] - x[-1]) 24 | if d < dhat: 25 | s = d / dhat 26 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 27 | # self-contact 28 | dhat_sqr = dhat * dhat 29 | for xI in bp: 30 | for eI in be: 31 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 32 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 33 | if d_sqr < dhat_sqr: 34 | s = d_sqr / dhat_sqr 35 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 36 | sum += 0.5 * contact_area[xI] * dhat * kappa / 8 * (s - 1) * math.log(s) 37 | return sum 38 | 39 | def grad(x, n, o, bp, be, contact_area): 40 | g = np.array([[0.0, 0.0]] * len(x)) 41 | # floor: 42 | for i in range(0, len(x)): 43 | d = n.dot(x[i] - o) 44 | if d < dhat: 45 | s = d / dhat 46 | g[i] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 47 | # ceil: 48 | n = np.array([0.0, -1.0]) 49 | for i in range(0, len(x) - 1): 50 | d = n.dot(x[i] - x[-1]) 51 | if d < dhat: 52 | s = d / dhat 53 | local_grad = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) * n 54 | g[i] += local_grad 55 | g[-1] -= local_grad 56 | # self-contact 57 | dhat_sqr = dhat * dhat 58 | for xI in bp: 59 | for eI in be: 60 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 61 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 62 | if d_sqr < dhat_sqr: 63 | s = d_sqr / dhat_sqr 64 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 65 | local_grad = 0.5 * contact_area[xI] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.grad(x[xI], x[eI[0]], x[eI[1]]) 66 | g[xI] += local_grad[0:2] 67 | g[eI[0]] += local_grad[2:4] 68 | g[eI[1]] += local_grad[4:6] 69 | return g 70 | 71 | def hess(x, n, o, bp, be, contact_area): 72 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 73 | # floor: 74 | for i in range(0, len(x)): 75 | d = n.dot(x[i] - o) 76 | if d < dhat: 77 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 78 | for c in range(0, 2): 79 | for r in range(0, 2): 80 | IJV[0].append(i * 2 + r) 81 | IJV[1].append(i * 2 + c) 82 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 83 | # ceil: 84 | n = np.array([0.0, -1.0]) 85 | for i in range(0, len(x) - 1): 86 | d = n.dot(x[i] - x[-1]) 87 | if d < dhat: 88 | local_hess = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) * np.outer(n, n) 89 | index = [i, len(x) - 1] 90 | for nI in range(0, 2): 91 | for nJ in range(0, 2): 92 | for c in range(0, 2): 93 | for r in range(0, 2): 94 | IJV[0].append(index[nI] * 2 + r) 95 | IJV[1].append(index[nJ] * 2 + c) 96 | IJV[2] = np.append(IJV[2], ((-1) ** (nI != nJ)) * local_hess[r, c]) 97 | # self-contact 98 | dhat_sqr = dhat * dhat 99 | for xI in bp: 100 | for eI in be: 101 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 102 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 103 | if d_sqr < dhat_sqr: 104 | d_sqr_grad = PE.grad(x[xI], x[eI[0]], x[eI[1]]) 105 | s = d_sqr / dhat_sqr 106 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity: 107 | local_hess = 0.5 * contact_area[xI] * dhat * utils.make_PSD(kappa / (8 * d_sqr * d_sqr * dhat_sqr) * (d_sqr + dhat_sqr) * np.outer(d_sqr_grad, d_sqr_grad) \ 108 | + (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * PE.hess(x[xI], x[eI[0]], x[eI[1]])) 109 | index = [xI, eI[0], eI[1]] 110 | for nI in range(0, 3): 111 | for nJ in range(0, 3): 112 | for c in range(0, 2): 113 | for r in range(0, 2): 114 | IJV[0].append(index[nI] * 2 + r) 115 | IJV[1].append(index[nJ] * 2 + c) 116 | IJV[2] = np.append(IJV[2], local_hess[nI * 2 + r, nJ * 2 + c]) 117 | return IJV 118 | 119 | def init_step_size(x, n, o, bp, be, p): 120 | alpha = 1 121 | # floor: 122 | for i in range(0, len(x)): 123 | p_n = p[i].dot(n) 124 | if p_n < 0: 125 | alpha = min(alpha, 0.9 * n.dot(x[i] - o) / -p_n) 126 | # ceil: 127 | n = np.array([0.0, -1.0]) 128 | for i in range(0, len(x) - 1): 129 | p_n = (p[i] - p[-1]).dot(n) 130 | if p_n < 0: 131 | alpha = min(alpha, 0.9 * n.dot(x[i] - x[-1]) / -p_n) 132 | # self-contact 133 | for xI in bp: 134 | for eI in be: 135 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 136 | if CCD.bbox_overlap(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha): 137 | toc = CCD.narrow_phase_CCD(x[xI], x[eI[0]], x[eI[1]], p[xI], p[eI[0]], p[eI[1]], alpha) 138 | if alpha > toc: 139 | alpha = toc 140 | return alpha 141 | 142 | def compute_mu_lambda(x, n, o, bp, be, contact_area, mu): 143 | # floor: 144 | mu_lambda = np.array([0.0] * len(x)) 145 | for i in range(0, len(x)): 146 | d = n.dot(x[i] - o) 147 | if d < dhat: 148 | s = d / dhat 149 | mu_lambda[i] = mu * -contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 150 | # ANCHOR: fric_precomp 151 | # self-contact 152 | mu_lambda_self = [] 153 | dhat_sqr = dhat * dhat 154 | for xI in bp: 155 | for eI in be: 156 | if xI != eI[0] and xI != eI[1]: # do not consider a point and its incident edge 157 | d_sqr = PE.val(x[xI], x[eI[0]], x[eI[1]]) 158 | if d_sqr < dhat_sqr: 159 | s = d_sqr / dhat_sqr 160 | # since d_sqr is used, need to divide by 8 not 2 here for consistency to linear elasticity 161 | # also, lambda = -\partial b / \partial d = -(\partial b / \partial d^2) * (\partial d^2 / \partial d) 162 | mu_lam = mu * -0.5 * contact_area[xI] * dhat * (kappa / 8 * (math.log(s) / dhat_sqr + (s - 1) / d_sqr)) * 2 * math.sqrt(d_sqr) 163 | [n, r] = PE.tangent(x[xI], x[eI[0]], x[eI[1]]) # normal and closest point parameterization on the edge 164 | mu_lambda_self.append([xI, eI[0], eI[1], mu_lam, n, r]) 165 | # ANCHOR_END: fric_precomp 166 | return [mu_lambda, mu_lambda_self] -------------------------------------------------------------------------------- /8_self_friction/FrictionEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import utils 3 | 4 | epsv = 1e-3 5 | 6 | def f0(vbarnorm, epsv, hhat): 7 | if vbarnorm >= epsv: 8 | return vbarnorm * hhat 9 | else: 10 | vbarnormhhat = vbarnorm * hhat 11 | epsvhhat = epsv * hhat 12 | return vbarnormhhat * vbarnormhhat * (-vbarnormhhat / 3.0 + epsvhhat) / (epsvhhat * epsvhhat) + epsvhhat / 3.0 13 | 14 | def f1_div_vbarnorm(vbarnorm, epsv): 15 | if vbarnorm >= epsv: 16 | return 1.0 / vbarnorm 17 | else: 18 | return (-vbarnorm + 2.0 * epsv) / (epsv * epsv) 19 | 20 | def f_hess_term(vbarnorm, epsv): 21 | if vbarnorm >= epsv: 22 | return -1.0 / (vbarnorm * vbarnorm) 23 | else: 24 | return -1.0 / (epsv * epsv) 25 | 26 | def val(v, mu_lambda, mu_lambda_self, hhat, n): 27 | sum = 0.0 28 | # floor: 29 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 30 | for i in range(0, len(v)): 31 | if mu_lambda[i] > 0: 32 | vbar = np.transpose(T).dot(v[i]) 33 | sum += mu_lambda[i] * f0(np.linalg.norm(vbar), epsv, hhat) 34 | # ANCHOR: val 35 | # self-contact: 36 | for i in range(0, len(mu_lambda_self)): 37 | [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i] 38 | T = np.identity(2) - np.outer(n, n) 39 | rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1]) 40 | vbar = np.transpose(T).dot(rel_v) 41 | sum += mu_lam * f0(np.linalg.norm(vbar), epsv, hhat) 42 | # ANCHOR_END: val 43 | return sum 44 | 45 | def grad(v, mu_lambda, mu_lambda_self, hhat, n): 46 | g = np.array([[0.0, 0.0]] * len(v)) 47 | # floor: 48 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 49 | for i in range(0, len(v)): 50 | if mu_lambda[i] > 0: 51 | vbar = np.transpose(T).dot(v[i]) 52 | g[i] = mu_lambda[i] * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 53 | # ANCHOR: grad 54 | # self-contact: 55 | for i in range(0, len(mu_lambda_self)): 56 | [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i] 57 | T = np.identity(2) - np.outer(n, n) 58 | rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1]) 59 | vbar = np.transpose(T).dot(rel_v) 60 | g_rel_v = mu_lam * f1_div_vbarnorm(np.linalg.norm(vbar), epsv) * T.dot(vbar) 61 | g[xI] += g_rel_v 62 | g[eI0] += g_rel_v * -(1 - r) 63 | g[eI1] += g_rel_v * -r 64 | # ANCHOR_END: grad 65 | return g 66 | 67 | def hess(v, mu_lambda, mu_lambda_self, hhat, n): 68 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 69 | # floor: 70 | T = np.identity(2) - np.outer(n, n) # tangent of slope is constant 71 | for i in range(0, len(v)): 72 | if mu_lambda[i] > 0: 73 | vbar = np.transpose(T).dot(v[i]) 74 | vbarnorm = np.linalg.norm(vbar) 75 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 76 | if vbarnorm != 0: 77 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 78 | local_hess = mu_lambda[i] * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 79 | for c in range(0, 2): 80 | for r in range(0, 2): 81 | IJV[0].append(i * 2 + r) 82 | IJV[1].append(i * 2 + c) 83 | IJV[2] = np.append(IJV[2], local_hess[r, c]) 84 | # ANCHOR: hess 85 | # self-contact: 86 | for i in range(0, len(mu_lambda_self)): 87 | [xI, eI0, eI1, mu_lam, n, r] = mu_lambda_self[i] 88 | T = np.identity(2) - np.outer(n, n) 89 | rel_v = v[xI] - ((1 - r) * v[eI0] + r * v[eI1]) 90 | vbar = np.transpose(T).dot(rel_v) 91 | vbarnorm = np.linalg.norm(vbar) 92 | inner_term = f1_div_vbarnorm(vbarnorm, epsv) * np.identity(2) 93 | if vbarnorm != 0: 94 | inner_term += f_hess_term(vbarnorm, epsv) / vbarnorm * np.outer(vbar, vbar) 95 | hess_rel_v = mu_lam * T.dot(utils.make_PSD(inner_term)).dot(np.transpose(T)) / hhat 96 | index = [xI, eI0, eI1] 97 | d_rel_v_dv = [1, -(1 - r), -r] 98 | for nI in range(0, 3): 99 | for nJ in range(0, 3): 100 | for c in range(0, 2): 101 | for r in range(0, 2): 102 | IJV[0].append(index[nI] * 2 + r) 103 | IJV[1].append(index[nJ] * 2 + c) 104 | IJV[2] = np.append(IJV[2], d_rel_v_dv[nI] * d_rel_v_dv[nJ] * hess_rel_v[r, c]) 105 | # ANCHOR_END: hess 106 | return IJV -------------------------------------------------------------------------------- /8_self_friction/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /8_self_friction/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /8_self_friction/NeoHookeanEnergy.py: -------------------------------------------------------------------------------- 1 | import utils 2 | import numpy as np 3 | import math 4 | 5 | def polar_svd(F): 6 | [U, s, VT] = np.linalg.svd(F) 7 | if np.linalg.det(U) < 0: 8 | U[:, 1] = -U[:, 1] 9 | s[1] = -s[1] 10 | if np.linalg.det(VT) < 0: 11 | VT[1, :] = -VT[1, :] 12 | s[1] = -s[1] 13 | return [U, s, VT] 14 | 15 | def dPsi_div_dsigma(s, mu, lam): 16 | ln_sigma_prod = math.log(s[0] * s[1]) 17 | inv0 = 1.0 / s[0] 18 | dPsi_dsigma_0 = mu * (s[0] - inv0) + lam * inv0 * ln_sigma_prod 19 | inv1 = 1.0 / s[1] 20 | dPsi_dsigma_1 = mu * (s[1] - inv1) + lam * inv1 * ln_sigma_prod 21 | return [dPsi_dsigma_0, dPsi_dsigma_1] 22 | 23 | def d2Psi_div_dsigma2(s, mu, lam): 24 | ln_sigma_prod = math.log(s[0] * s[1]) 25 | inv2_0 = 1 / (s[0] * s[0]) 26 | d2Psi_dsigma2_00 = mu * (1 + inv2_0) - lam * inv2_0 * (ln_sigma_prod - 1) 27 | inv2_1 = 1 / (s[1] * s[1]) 28 | d2Psi_dsigma2_11 = mu * (1 + inv2_1) - lam * inv2_1 * (ln_sigma_prod - 1) 29 | d2Psi_dsigma2_01 = lam / (s[0] * s[1]) 30 | return [[d2Psi_dsigma2_00, d2Psi_dsigma2_01], [d2Psi_dsigma2_01, d2Psi_dsigma2_11]] 31 | 32 | def B_left_coef(s, mu, lam): 33 | sigma_prod = s[0] * s[1] 34 | return (mu + (mu - lam * math.log(sigma_prod)) / sigma_prod) / 2 35 | 36 | def Psi(F, mu, lam): 37 | J = np.linalg.det(F) 38 | lnJ = math.log(J) 39 | return mu / 2 * (np.trace(np.transpose(F).dot(F)) - 2) - mu * lnJ + lam / 2 * lnJ * lnJ 40 | 41 | def dPsi_div_dF(F, mu, lam): 42 | FinvT = np.transpose(np.linalg.inv(F)) 43 | return mu * (F - FinvT) + lam * math.log(np.linalg.det(F)) * FinvT 44 | 45 | def d2Psi_div_dF2(F, mu, lam): 46 | [U, sigma, VT] = polar_svd(F) 47 | 48 | Psi_sigma_sigma = utils.make_PSD(d2Psi_div_dsigma2(sigma, mu, lam)) 49 | 50 | B_left = B_left_coef(sigma, mu, lam) 51 | Psi_sigma = dPsi_div_dsigma(sigma, mu, lam) 52 | B_right = (Psi_sigma[0] + Psi_sigma[1]) / (2 * max(sigma[0] + sigma[1], 1e-6)) 53 | B = utils.make_PSD([[B_left + B_right, B_left - B_right], [B_left - B_right, B_left + B_right]]) 54 | 55 | M = np.array([[0, 0, 0, 0]] * 4) 56 | M[0, 0] = Psi_sigma_sigma[0, 0] 57 | M[0, 3] = Psi_sigma_sigma[0, 1] 58 | M[1, 1] = B[0, 0] 59 | M[1, 2] = B[0, 1] 60 | M[2, 1] = B[1, 0] 61 | M[2, 2] = B[1, 1] 62 | M[3, 0] = Psi_sigma_sigma[1, 0] 63 | M[3, 3] = Psi_sigma_sigma[1, 1] 64 | 65 | dP_div_dF = np.array([[0, 0, 0, 0]] * 4) 66 | for j in range(0, 2): 67 | for i in range(0, 2): 68 | ij = j * 2 + i 69 | for s in range(0, 2): 70 | for r in range(0, 2): 71 | rs = s * 2 + r 72 | dP_div_dF[ij, rs] = M[0, 0] * U[i, 0] * VT[0, j] * U[r, 0] * VT[0, s] \ 73 | + M[0, 3] * U[i, 0] * VT[0, j] * U[r, 1] * VT[1, s] \ 74 | + M[1, 1] * U[i, 1] * VT[0, j] * U[r, 1] * VT[0, s] \ 75 | + M[1, 2] * U[i, 1] * VT[0, j] * U[r, 0] * VT[1, s] \ 76 | + M[2, 1] * U[i, 0] * VT[1, j] * U[r, 1] * VT[0, s] \ 77 | + M[2, 2] * U[i, 0] * VT[1, j] * U[r, 0] * VT[1, s] \ 78 | + M[3, 0] * U[i, 1] * VT[1, j] * U[r, 0] * VT[0, s] \ 79 | + M[3, 3] * U[i, 1] * VT[1, j] * U[r, 1] * VT[1, s] 80 | return dP_div_dF 81 | 82 | def deformation_grad(x, elemVInd, IB): 83 | F = [x[elemVInd[1]] - x[elemVInd[0]], x[elemVInd[2]] - x[elemVInd[0]]] 84 | return np.transpose(F).dot(IB) 85 | 86 | def dPsi_div_dx(P, IB): # applying chain-rule, dPsi_div_dx = dPsi_div_dF * dF_div_dx 87 | dPsi_dx_2 = P[0, 0] * IB[0, 0] + P[0, 1] * IB[0, 1] 88 | dPsi_dx_3 = P[1, 0] * IB[0, 0] + P[1, 1] * IB[0, 1] 89 | dPsi_dx_4 = P[0, 0] * IB[1, 0] + P[0, 1] * IB[1, 1] 90 | dPsi_dx_5 = P[1, 0] * IB[1, 0] + P[1, 1] * IB[1, 1] 91 | return [np.array([-dPsi_dx_2 - dPsi_dx_4, -dPsi_dx_3 - dPsi_dx_5]), np.array([dPsi_dx_2, dPsi_dx_3]), np.array([dPsi_dx_4, dPsi_dx_5])] 92 | 93 | def d2Psi_div_dx2(dP_div_dF, IB): # applying chain-rule, d2Psi_div_dx2 = dF_div_dx^T * d2Psi_div_dF2 * dF_div_dx (note that d2F_div_dx2 = 0) 94 | intermediate = np.array([[0.0, 0.0, 0.0, 0.0]] * 6) 95 | for colI in range(0, 4): 96 | _000 = dP_div_dF[0, colI] * IB[0, 0] 97 | _010 = dP_div_dF[0, colI] * IB[1, 0] 98 | _101 = dP_div_dF[2, colI] * IB[0, 1] 99 | _111 = dP_div_dF[2, colI] * IB[1, 1] 100 | _200 = dP_div_dF[1, colI] * IB[0, 0] 101 | _210 = dP_div_dF[1, colI] * IB[1, 0] 102 | _301 = dP_div_dF[3, colI] * IB[0, 1] 103 | _311 = dP_div_dF[3, colI] * IB[1, 1] 104 | intermediate[2, colI] = _000 + _101 105 | intermediate[3, colI] = _200 + _301 106 | intermediate[4, colI] = _010 + _111 107 | intermediate[5, colI] = _210 + _311 108 | intermediate[0, colI] = -intermediate[2, colI] - intermediate[4, colI] 109 | intermediate[1, colI] = -intermediate[3, colI] - intermediate[5, colI] 110 | result = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * 6) 111 | for colI in range(0, 6): 112 | _000 = intermediate[colI, 0] * IB[0, 0] 113 | _010 = intermediate[colI, 0] * IB[1, 0] 114 | _101 = intermediate[colI, 2] * IB[0, 1] 115 | _111 = intermediate[colI, 2] * IB[1, 1] 116 | _200 = intermediate[colI, 1] * IB[0, 0] 117 | _210 = intermediate[colI, 1] * IB[1, 0] 118 | _301 = intermediate[colI, 3] * IB[0, 1] 119 | _311 = intermediate[colI, 3] * IB[1, 1] 120 | result[2, colI] = _000 + _101 121 | result[3, colI] = _200 + _301 122 | result[4, colI] = _010 + _111 123 | result[5, colI] = _210 + _311 124 | result[0, colI] = -_000 - _101 - _010 - _111 125 | result[1, colI] = -_200 - _301 - _210 - _311 126 | return result 127 | 128 | def val(x, e, vol, IB, mu, lam): 129 | sum = 0.0 130 | for i in range(0, len(e)): 131 | F = deformation_grad(x, e[i], IB[i]) 132 | sum += vol[i] * Psi(F, mu[i], lam[i]) 133 | return sum 134 | 135 | def grad(x, e, vol, IB, mu, lam): 136 | g = np.array([[0.0, 0.0]] * len(x)) 137 | for i in range(0, len(e)): 138 | F = deformation_grad(x, e[i], IB[i]) 139 | P = vol[i] * dPsi_div_dF(F, mu[i], lam[i]) 140 | g_local = dPsi_div_dx(P, IB[i]) 141 | for j in range(0, 3): 142 | g[e[i][j]] += g_local[j] 143 | return g 144 | 145 | def hess(x, e, vol, IB, mu, lam): 146 | IJV = [[0] * (len(e) * 36), [0] * (len(e) * 36), np.array([0.0] * (len(e) * 36))] 147 | for i in range(0, len(e)): 148 | F = deformation_grad(x, e[i], IB[i]) 149 | dP_div_dF = vol[i] * d2Psi_div_dF2(F, mu[i], lam[i]) 150 | local_hess = d2Psi_div_dx2(dP_div_dF, IB[i]) 151 | for xI in range(0, 3): 152 | for xJ in range(0, 3): 153 | for dI in range(0, 2): 154 | for dJ in range(0, 2): 155 | ind = i * 36 + (xI * 3 + xJ) * 4 + dI * 2 + dJ 156 | IJV[0][ind] = e[i][xI] * 2 + dI 157 | IJV[1][ind] = e[i][xJ] * 2 + dJ 158 | IJV[2][ind] = local_hess[xI * 2 + dI, xJ * 2 + dJ] 159 | return IJV 160 | 161 | def init_step_size(x, e, p): 162 | alpha = 1 163 | for i in range(0, len(e)): 164 | x21 = x[e[i][1]] - x[e[i][0]] 165 | x31 = x[e[i][2]] - x[e[i][0]] 166 | p21 = p[e[i][1]] - p[e[i][0]] 167 | p31 = p[e[i][2]] - p[e[i][0]] 168 | detT = np.linalg.det(np.transpose([x21, x31])) 169 | a = np.linalg.det(np.transpose([p21, p31])) / detT 170 | b = (np.linalg.det(np.transpose([x21, p31])) + np.linalg.det(np.transpose([p21, x31]))) / detT 171 | c = 0.9 # solve for alpha that first brings the new volume to 0.1x the old volume for slackness 172 | critical_alpha = utils.smallest_positive_real_root_quad(a, b, c) 173 | if critical_alpha > 0: 174 | alpha = min(alpha, critical_alpha) 175 | return alpha -------------------------------------------------------------------------------- /8_self_friction/SpringEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, m, DBC, DBC_target, k): 4 | sum = 0.0 5 | for i in range(0, len(DBC)): 6 | diff = x[DBC[i]] - DBC_target[i] 7 | sum += 0.5 * k * m[DBC[i]] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, m, DBC, DBC_target, k): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(DBC)): 13 | g[DBC[i]] = k * m[DBC[i]] * (x[DBC[i]] - DBC_target[i]) 14 | return g 15 | 16 | def hess(x, m, DBC, DBC_target, k): 17 | IJV = [[0] * 0, [0] * 0, np.array([0.0] * 0)] 18 | for i in range(0, len(DBC)): 19 | for d in range(0, 2): 20 | IJV[0].append(DBC[i] * 2 + d) 21 | IJV[1].append(DBC[i] * 2 + d) 22 | IJV[2] = np.append(IJV[2], k * m[DBC[i]]) 23 | return IJV -------------------------------------------------------------------------------- /8_self_friction/distance/CCD.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | import numpy as np 3 | import math 4 | 5 | import distance.PointEdgeDistance as PE 6 | 7 | # check whether the bounding box of the trajectory of the point and the edge overlap 8 | def bbox_overlap(p, e0, e1, dp, de0, de1, toc_upperbound): 9 | max_p = np.maximum(p, p + toc_upperbound * dp) # point trajectory bbox top-right 10 | min_p = np.minimum(p, p + toc_upperbound * dp) # point trajectory bbox bottom-left 11 | max_e = np.maximum(np.maximum(e0, e0 + toc_upperbound * de0), np.maximum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox top-right 12 | min_e = np.minimum(np.minimum(e0, e0 + toc_upperbound * de0), np.minimum(e1, e1 + toc_upperbound * de1)) # edge trajectory bbox bottom-left 13 | if np.any(np.greater(min_p, max_e)) or np.any(np.greater(min_e, max_p)): 14 | return False 15 | else: 16 | return True 17 | 18 | # compute the first "time" of contact, or toc, 19 | # return the computed toc only if it is smaller than the previously computed toc_upperbound 20 | def narrow_phase_CCD(_p, _e0, _e1, _dp, _de0, _de1, toc_upperbound): 21 | p = deepcopy(_p) 22 | e0 = deepcopy(_e0) 23 | e1 = deepcopy(_e1) 24 | dp = deepcopy(_dp) 25 | de0 = deepcopy(_de0) 26 | de1 = deepcopy(_de1) 27 | 28 | # use relative displacement for faster convergence 29 | mov = (dp + de0 + de1) / 3 30 | de0 -= mov 31 | de1 -= mov 32 | dp -= mov 33 | maxDispMag = np.linalg.norm(dp) + math.sqrt(max(np.dot(de0, de0), np.dot(de1, de1))) 34 | if maxDispMag == 0: 35 | return toc_upperbound 36 | 37 | eta = 0.1 # calculate the toc that first brings the distance to 0.1x the current distance 38 | dist2_cur = PE.val(p, e0, e1) 39 | dist_cur = math.sqrt(dist2_cur) 40 | gap = eta * dist_cur 41 | # iteratively move the point and edge towards each other and 42 | # grow the toc estimate without numerical errors 43 | toc = 0 44 | while True: 45 | tocLowerBound = (1 - eta) * dist_cur / maxDispMag 46 | 47 | p += tocLowerBound * dp 48 | e0 += tocLowerBound * de0 49 | e1 += tocLowerBound * de1 50 | dist2_cur = PE.val(p, e0, e1) 51 | dist_cur = math.sqrt(dist2_cur) 52 | if toc != 0 and dist_cur < gap: 53 | break 54 | 55 | toc += tocLowerBound 56 | if toc > toc_upperbound: 57 | return toc_upperbound 58 | 59 | return toc -------------------------------------------------------------------------------- /8_self_friction/distance/PointEdgeDistance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import distance.PointPointDistance as PP 4 | import distance.PointLineDistance as PL 5 | 6 | def val(p, e0, e1): 7 | e = e1 - e0 8 | ratio = np.dot(e, p - e0) / np.dot(e, e) 9 | if ratio < 0: # point(p)-point(e0) expression 10 | return PP.val(p, e0) 11 | elif ratio > 1: # point(p)-point(e1) expression 12 | return PP.val(p, e1) 13 | else: # point(p)-line(e0e1) expression 14 | return PL.val(p, e0, e1) 15 | 16 | def grad(p, e0, e1): 17 | e = e1 - e0 18 | ratio = np.dot(e, p - e0) / np.dot(e, e) 19 | if ratio < 0: # point(p)-point(e0) expression 20 | g_PP = PP.grad(p, e0) 21 | return np.reshape([g_PP[0:2], g_PP[2:4], np.array([0.0, 0.0])], (1, 6))[0] 22 | elif ratio > 1: # point(p)-point(e1) expression 23 | g_PP = PP.grad(p, e1) 24 | return np.reshape([g_PP[0:2], np.array([0.0, 0.0]), g_PP[2:4]], (1, 6))[0] 25 | else: # point(p)-line(e0e1) expression 26 | return PL.grad(p, e0, e1) 27 | 28 | def hess(p, e0, e1): 29 | e = e1 - e0 30 | ratio = np.dot(e, p - e0) / np.dot(e, e) 31 | if ratio < 0: # point(p)-point(e0) expression 32 | H_PP = PP.hess(p, e0) 33 | return np.array([np.reshape([H_PP[0, 0:2], H_PP[0, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 34 | np.reshape([H_PP[1, 0:2], H_PP[1, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 35 | np.reshape([H_PP[2, 0:2], H_PP[2, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 36 | np.reshape([H_PP[3, 0:2], H_PP[3, 2:4], np.array([0.0, 0.0])], (1, 6))[0], \ 37 | np.array([0.0] * 6), \ 38 | np.array([0.0] * 6)]) 39 | elif ratio > 1: # point(p)-point(e1) expression 40 | H_PP = PP.hess(p, e1) 41 | return np.array([np.reshape([H_PP[0, 0:2], np.array([0.0, 0.0]), H_PP[0, 2:4]], (1, 6))[0], \ 42 | np.reshape([H_PP[1, 0:2], np.array([0.0, 0.0]), H_PP[1, 2:4]], (1, 6))[0], \ 43 | np.array([0.0] * 6), \ 44 | np.array([0.0] * 6), \ 45 | np.reshape([H_PP[2, 0:2], np.array([0.0, 0.0]), H_PP[2, 2:4]], (1, 6))[0], \ 46 | np.reshape([H_PP[3, 0:2], np.array([0.0, 0.0]), H_PP[3, 2:4]], (1, 6))[0]]) 47 | else: # point(p)-line(e0e1) expression 48 | return PL.hess(p, e0, e1) 49 | 50 | # ANCHOR: tangent 51 | # compute normal and the parameterization of the closest point on the edge 52 | def tangent(p, e0, e1): 53 | e = e1 - e0 54 | ratio = np.dot(e, p - e0) / np.dot(e, e) 55 | if ratio < 0: # point(p)-point(e0) expression 56 | n = p - e0 57 | elif ratio > 1: # point(p)-point(e1) expression 58 | n = p - e1 59 | else: # point(p)-line(e0e1) expression 60 | n = p - ((1 - ratio) * e0 + ratio * e1) 61 | return [n / np.linalg.norm(n), ratio] 62 | # ANCHOR_END: tangent -------------------------------------------------------------------------------- /8_self_friction/distance/PointLineDistance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(p, e0, e1): 4 | e = e1 - e0 5 | numerator = e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0] 6 | return numerator * numerator / np.dot(e, e) 7 | 8 | def grad(p, e0, e1): 9 | g = np.array([0.0] * 6) 10 | t13 = -e1[0] + e0[0] 11 | t14 = -e1[1] + e0[1] 12 | t23 = 1.0 / (t13 * t13 + t14 * t14) 13 | t25 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t14 * p[0]) + -(t13 * p[1]) 14 | t24 = t23 * t23 15 | t26 = t25 * t25 16 | t27 = (e0[0] * 2.0 + -(e1[0] * 2.0)) * t24 * t26 17 | t26 *= (e0[1] * 2.0 + -(e1[1] * 2.0)) * t24 18 | g[0] = t14 * t23 * t25 * 2.0 19 | g[1] = t13 * t23 * t25 * -2.0 20 | t24 = t23 * t25 21 | g[2] = -t27 - t24 * (-e1[1] + p[1]) * 2.0 22 | g[3] = -t26 + t24 * (-e1[0] + p[0]) * 2.0 23 | g[4] = t27 + t24 * (p[1] - e0[1]) * 2.0 24 | g[5] = t26 - t24 * (p[0] - e0[0]) * 2.0 25 | return g 26 | 27 | def hess(p, e0, e1): 28 | H = np.array([0.0] * 36) 29 | t15 = -e0[0] + p[0] 30 | t16 = -e0[1] + p[1] 31 | t17 = -e1[0] + p[0] 32 | t18 = -e1[1] + p[1] 33 | t19 = -e1[0] + e0[0] 34 | t20 = -e1[1] + e0[1] 35 | t21 = e0[0] * 2.0 + -(e1[0] * 2.0) 36 | t22 = e0[1] * 2.0 + -(e1[1] * 2.0) 37 | t23 = t19 * t19 38 | t24 = t20 * t20 39 | t31 = 1.0 / (t23 + t24) 40 | t34 = ((e0[0] * e1[1] + -(e0[1] * e1[0])) + t20 * p[0]) + -(t19 * p[1]) 41 | t32 = t31 * t31 42 | t33 = t32 * t31 43 | t35 = t34 * t34 44 | t60 = t31 * t34 * 2.0 45 | t59 = -(t19 * t20 * t31 * 2.0) 46 | t62 = t32 * t35 * 2.0 47 | t64 = t21 * t21 * t33 * t35 * 2.0 48 | t65 = t22 * t22 * t33 * t35 * 2.0 49 | t68 = t15 * t21 * t32 * t34 * 2.0 50 | t71 = t16 * t22 * t32 * t34 * 2.0 51 | t72 = t17 * t21 * t32 * t34 * 2.0 52 | t75 = t18 * t22 * t32 * t34 * 2.0 53 | t76 = t19 * t21 * t32 * t34 * 2.0 54 | t77 = t20 * t21 * t32 * t34 * 2.0 55 | t78 = t19 * t22 * t32 * t34 * 2.0 56 | t79 = t20 * t22 * t32 * t34 * 2.0 57 | t90 = t21 * t22 * t33 * t35 * 2.0 58 | t92 = t16 * t20 * t31 * 2.0 + t77 59 | t94 = -(t17 * t19 * t31 * 2.0) + t78 60 | t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76 61 | t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79 62 | t93 = t15 * t19 * t31 * 2.0 + -t78 63 | t35 = -(t18 * t20 * t31 * 2.0) + -t77 64 | t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79 65 | t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76 66 | t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90 67 | t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90 68 | t102_tmp = t17 * t22 * t32 * t34 69 | t76 = t15 * t22 * t32 * t34 70 | t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0) + t102_tmp * 2.0 71 | t33 = t18 * t21 * t32 * t34 72 | t20 = t16 * t21 * t32 * t34 73 | t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0)) + -(t33 * 2.0) 74 | t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90 75 | t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90 76 | H[0] = t24 * t31 * 2.0 77 | H[1] = t59 78 | H[2] = t35 79 | H[3] = t97 80 | H[4] = t92 81 | H[5] = t99 82 | H[6] = t59 83 | H[7] = t23 * t31 * 2.0 84 | H[8] = t96 85 | H[9] = t94 86 | H[10] = t98 87 | H[11] = t93 88 | H[12] = t35 89 | H[13] = t96 90 | t35 = -t62 + t64 91 | H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0 92 | H[15] = t19 93 | H[16] = t79 94 | H[17] = t77 95 | H[18] = t97 96 | H[19] = t94 97 | H[20] = t19 98 | t33 = -t62 + t65 99 | H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0 100 | H[22] = t78 101 | H[23] = t22 102 | H[24] = t92 103 | H[25] = t98 104 | H[26] = t79 105 | H[27] = t78 106 | H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0 107 | H[29] = t100 108 | H[30] = t99 109 | H[31] = t93 110 | H[32] = t77 111 | H[33] = t22 112 | H[34] = t100 113 | H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0 114 | return np.reshape(H, (6, 6)) -------------------------------------------------------------------------------- /8_self_friction/distance/PointPointDistance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(p0, p1): 4 | e = p0 - p1 5 | return np.dot(e, e) 6 | 7 | def grad(p0, p1): 8 | e = p0 - p1 9 | return np.reshape([2 * e, -2 * e], (1, 4))[0] 10 | 11 | def hess(p0, p1): 12 | H = np.array([[0.0] * 4] * 4) 13 | H[0, 0] = H[1, 1] = H[2, 2] = H[3, 3] = 2 14 | H[0, 2] = H[1, 3] = H[2, 0] = H[3, 1] = -2 15 | return H -------------------------------------------------------------------------------- /8_self_friction/readme.md: -------------------------------------------------------------------------------- 1 | # Inversion-free Hyperelastic Solids Simulation 2 | 3 | Two squares falling onto the ground under gravity, contacting with each other, and then compressed by a ceiling is simulated with an inversion-free hyperelastic potential and IPC with implicit Euler time integration. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ## Dependencies 7 | ``` 8 | pip install numpy scipy pygame 9 | ``` 10 | 11 | ## Run 12 | ``` 13 | python simulator.py 14 | ``` -------------------------------------------------------------------------------- /8_self_friction/simulator.py: -------------------------------------------------------------------------------- 1 | # FEM Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import square_mesh # square mesh 8 | import time_integrator 9 | 10 | # simulation setup 11 | side_len = 0.45 12 | rho = 1000 # density of square 13 | E = 1e5 # Young's modulus 14 | nu = 0.4 # Poisson's ratio 15 | n_seg = 2 # num of segments per side of the square 16 | h = 0.01 # time step size in s 17 | DBC = [(n_seg + 1) * (n_seg + 1) * 2] # dirichlet node index 18 | DBC_v = [np.array([0.0, -0.5])] # dirichlet node velocity 19 | DBC_limit = [np.array([0.0, -0.7])] # dirichlet node limit position 20 | ground_n = np.array([0.0, 1.0]) # normal of the slope 21 | ground_n /= np.linalg.norm(ground_n) # normalize ground normal vector just in case 22 | ground_o = np.array([0.0, -1.0]) # a point on the slope 23 | mu = 0.4 # friction coefficient of the slope 24 | 25 | # initialize simulation 26 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and triangle node indices of the top square 27 | e = np.append(e, np.array(e) + [len(x)] * 3, axis=0) # add triangle node indices of the bottom square 28 | x = np.append(x, x + [side_len * 0.1, -side_len * 1.1], axis=0) # add node positions of the bottom square 29 | [bp, be] = square_mesh.find_boundary(e) # find boundary points and edges for self-contact 30 | x = np.append(x, [[0.0, side_len * 0.6]], axis=0) # ceil origin (with normal [0.0, -1.0]) 31 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 32 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 33 | # rest shape basis, volume, and lame parameters 34 | vol = [0.0] * len(e) 35 | IB = [np.array([[0.0, 0.0]] * 2)] * len(e) 36 | for i in range(0, len(e)): 37 | TB = [x[e[i][1]] - x[e[i][0]], x[e[i][2]] - x[e[i][0]]] 38 | vol[i] = np.linalg.det(np.transpose(TB)) / 2 39 | IB[i] = np.linalg.inv(np.transpose(TB)) 40 | mu_lame = [0.5 * E / (1 + nu)] * len(e) 41 | lam = [E * nu / ((1 + nu) * (1 - 2 * nu))] * len(e) 42 | # identify whether a node is Dirichlet 43 | is_DBC = [False] * len(x) 44 | for i in DBC: 45 | is_DBC[i] = True 46 | DBC_stiff = [1000] # DBC stiffness, adjusted and warm-started across time steps 47 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 48 | 49 | # simulation with visualization 50 | resolution = np.array([900, 900]) 51 | offset = resolution / 2 52 | scale = 200 53 | def screen_projection(x): 54 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 55 | 56 | time_step = 0 57 | square_mesh.write_to_file(time_step, x, e) 58 | screen = pygame.display.set_mode(resolution) 59 | running = True 60 | while running: 61 | # run until the user asks to quit 62 | for event in pygame.event.get(): 63 | if event.type == pygame.QUIT: 64 | running = False 65 | 66 | print('### Time step', time_step, '###') 67 | 68 | # fill the background and draw the square 69 | screen.fill((255, 255, 255)) 70 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([ground_o[0] - 3.0 * ground_n[1], ground_o[1] + 3.0 * ground_n[0]]), 71 | screen_projection([ground_o[0] + 3.0 * ground_n[1], ground_o[1] - 3.0 * ground_n[0]])) # ground 72 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([x[-1][0] + 3.0, x[-1][1]]), 73 | screen_projection([x[-1][0] - 3.0, x[-1][1]])) # ceil 74 | for eI in e: 75 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 76 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[1]]), screen_projection(x[eI[2]])) 77 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[2]]), screen_projection(x[eI[0]])) 78 | for xId in range(0, len(x) - 1): 79 | xI = x[xId] 80 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 81 | 82 | pygame.display.flip() # flip the display 83 | 84 | # step forward simulation and wait for screen refresh 85 | [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, ground_n, ground_o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, 1e-2) 86 | time_step += 1 87 | pygame.time.wait(int(h * 1000)) 88 | square_mesh.write_to_file(time_step, x, e) 89 | 90 | pygame.quit() -------------------------------------------------------------------------------- /8_self_friction/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # connect the nodes with triangle elements 13 | e = [] 14 | for i in range(0, n_seg): 15 | for j in range(0, n_seg): 16 | # triangulate each cell following a symmetric pattern: 17 | if (i % 2)^(j % 2): 18 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 19 | e.append([(i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 20 | else: 21 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 22 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 23 | 24 | return [x, e] 25 | 26 | def find_boundary(e): 27 | # index all half-edges for fast query 28 | edge_set = set() 29 | for i in range(0, len(e)): 30 | for j in range(0, 3): 31 | edge_set.add((e[i][j], e[i][(j + 1) % 3])) 32 | 33 | # find boundary points and edges 34 | bp_set = set() 35 | be = [] 36 | for eI in edge_set: 37 | if (eI[1], eI[0]) not in edge_set: 38 | # if the inverse edge of a half-edge does not exist, 39 | # then it is a boundary edge 40 | be.append([eI[0], eI[1]]) 41 | bp_set.add(eI[0]) 42 | bp_set.add(eI[1]) 43 | return [list(bp_set), be] 44 | 45 | def write_to_file(frameNum, x, e): 46 | # Check if 'output' directory exists; if not, create it 47 | if not os.path.exists('output'): 48 | os.makedirs('output') 49 | 50 | # create obj file 51 | filename = f"output/{frameNum}.obj" 52 | with open(filename, 'w') as f: 53 | # write vertex coordinates 54 | for row in x: 55 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 56 | # write vertex indices for each triangle 57 | for row in e: 58 | #NOTE: vertex indices start from 1 in obj file format 59 | f.write(f"f {row[0] + 1} {row[1] + 1} {row[2] + 1}\n") -------------------------------------------------------------------------------- /8_self_friction/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import NeoHookeanEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | import FrictionEnergy 14 | import SpringEnergy 15 | 16 | def step_forward(x, e, v, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, mu, is_DBC, DBC, DBC_v, DBC_limit, DBC_stiff, h, tol): 17 | x_tilde = x + v * h # implicit Euler predictive position 18 | x_n = copy.deepcopy(x) 19 | [mu_lambda, mu_lambda_self] = BarrierEnergy.compute_mu_lambda(x, n, o, bp, be, contact_area, mu) # compute mu * lambda for each node using x^n 20 | DBC_target = [] # target position of each DBC in the current time step 21 | for i in range(0, len(DBC)): 22 | if (DBC_limit[i] - x_n[DBC[i]]).dot(DBC_v[i]) > 0: 23 | DBC_target.append(x_n[DBC[i]] + h * DBC_v[i]) 24 | else: 25 | DBC_target.append(x_n[DBC[i]]) 26 | 27 | # Newton loop 28 | iter = 0 29 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff[0], h) 30 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 31 | while (LA.norm(p, inf) / h > tol) | (sum(DBC_satisfied) != len(DBC)): # also check whether all DBCs are satisfied 32 | print('Iteration', iter, ':') 33 | print('residual =', LA.norm(p, inf) / h) 34 | 35 | if (LA.norm(p, inf) / h <= tol) & (sum(DBC_satisfied) != len(DBC)): 36 | # increase DBC stiffness and recompute energy value record 37 | DBC_stiff[0] *= 2 38 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff[0], h) 39 | 40 | # filter line search 41 | alpha = min(BarrierEnergy.init_step_size(x, n, o, bp, be, p), NeoHookeanEnergy.init_step_size(x, e, p)) # avoid interpenetration, tunneling, and inversion 42 | while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x + alpha * p - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff[0], h) > E_last: 43 | alpha /= 2 44 | print('step size =', alpha) 45 | 46 | x += alpha * p 47 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff[0], h) 48 | [p, DBC_satisfied] = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, (x - x_n) / h, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff[0], tol, h) 49 | iter += 1 50 | 51 | v = (x - x_n) / h # implicit Euler velocity update 52 | return [x, v] 53 | 54 | def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h): 55 | return InertiaEnergy.val(x, x_tilde, m) + h * h * ( # implicit Euler 56 | NeoHookeanEnergy.val(x, e, vol, IB, mu_lame, lam) + 57 | GravityEnergy.val(x, m) + 58 | BarrierEnergy.val(x, n, o, bp, be, contact_area) + 59 | FrictionEnergy.val(v, mu_lambda, mu_lambda_self, h, n) 60 | ) + SpringEnergy.val(x, m, DBC, DBC_target, DBC_stiff) 61 | 62 | def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h): 63 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * ( # implicit Euler 64 | NeoHookeanEnergy.grad(x, e, vol, IB, mu_lame, lam) + 65 | GravityEnergy.grad(x, m) + 66 | BarrierEnergy.grad(x, n, o, bp, be, contact_area) + 67 | FrictionEnergy.grad(v, mu_lambda, mu_lambda_self, h, n) 68 | ) + SpringEnergy.grad(x, m, DBC, DBC_target, DBC_stiff) 69 | 70 | def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h): 71 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 72 | IJV_MS = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam) 73 | IJV_B = BarrierEnergy.hess(x, n, o, bp, be, contact_area) 74 | IJV_F = FrictionEnergy.hess(v, mu_lambda, mu_lambda_self, h, n) 75 | IJV_S = SpringEnergy.hess(x, m, DBC, DBC_target, DBC_stiff) 76 | IJV_MS[2] *= h * h # implicit Euler 77 | IJV_B[2] *= h * h # implicit Euler 78 | IJV_F[2] *= h * h # implicit Euler 79 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 80 | IJV_In_MS_B = np.append(IJV_In_MS, IJV_B, axis=1) 81 | IJV_In_MS_B_F = np.append(IJV_In_MS_B, IJV_F, axis=1) 82 | IJV = np.append(IJV_In_MS_B_F, IJV_S, axis=1) 83 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 84 | return H 85 | 86 | def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, is_DBC, DBC, DBC_target, DBC_stiff, tol, h): 87 | projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h) 88 | reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, n, o, bp, be, contact_area, v, mu_lambda, mu_lambda_self, DBC, DBC_target, DBC_stiff, h).reshape(len(x) * 2, 1) 89 | # check whether each DBC is satisfied 90 | DBC_satisfied = [False] * len(x) 91 | for i in range(0, len(DBC)): 92 | if LA.norm(x[DBC[i]] - DBC_target[i]) / h < tol: 93 | DBC_satisfied[DBC[i]] = True 94 | # eliminate DOF if it's a satisfied DBC by modifying gradient and Hessian for DBC: 95 | for i, j in zip(*projected_hess.nonzero()): 96 | if (is_DBC[int(i / 2)] & DBC_satisfied[int(i / 2)]) | (is_DBC[int(j / 2)] & DBC_satisfied[int(j / 2)]): 97 | projected_hess[i, j] = (i == j) 98 | for i in range(0, len(x)): 99 | if is_DBC[i] & DBC_satisfied[i]: 100 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 101 | return [spsolve(projected_hess, -reshaped_grad).reshape(len(x), 2), DBC_satisfied] -------------------------------------------------------------------------------- /8_self_friction/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | import math 4 | 5 | def make_PSD(hess): 6 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 7 | # set all negative Eigenvalues to 0 8 | for i in range(0, len(lam)): 9 | lam[i] = max(0, lam[i]) 10 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) 11 | 12 | def smallest_positive_real_root_quad(a, b, c, tol = 1e-6): 13 | # return negative value if no positive real root is found 14 | t = 0 15 | if abs(a) <= tol: 16 | if abs(b) <= tol: # f(x) = c > 0 for all x 17 | t = -1 18 | else: 19 | t = -c / b 20 | else: 21 | desc = b * b - 4 * a * c 22 | if desc > 0: 23 | t = (-b - math.sqrt(desc)) / (2 * a) 24 | if t < 0: 25 | t = (-b + math.sqrt(desc)) / (2 * a) 26 | else: # desv<0 ==> imag, f(x) > 0 for all x > 0 27 | t = -1 28 | return t -------------------------------------------------------------------------------- /9_reduced_DOF/BarrierEnergy.py: -------------------------------------------------------------------------------- 1 | # ANCHOR: val_grad_hess 2 | import math 3 | import numpy as np 4 | 5 | dhat = 0.01 6 | kappa = 1e5 7 | 8 | def val(x, y_ground, contact_area): 9 | sum = 0.0 10 | for i in range(0, len(x)): 11 | d = x[i][1] - y_ground 12 | if d < dhat: 13 | s = d / dhat 14 | sum += contact_area[i] * dhat * kappa / 2 * (s - 1) * math.log(s) 15 | return sum 16 | 17 | def grad(x, y_ground, contact_area): 18 | g = np.array([[0.0, 0.0]] * len(x)) 19 | for i in range(0, len(x)): 20 | d = x[i][1] - y_ground 21 | if d < dhat: 22 | s = d / dhat 23 | g[i][1] = contact_area[i] * dhat * (kappa / 2 * (math.log(s) / dhat + (s - 1) / d)) 24 | return g 25 | 26 | def hess(x, y_ground, contact_area): 27 | IJV = [[0] * len(x), [0] * len(x), np.array([0.0] * len(x))] 28 | for i in range(0, len(x)): 29 | IJV[0][i] = i * 2 + 1 30 | IJV[1][i] = i * 2 + 1 31 | d = x[i][1] - y_ground 32 | if d < dhat: 33 | IJV[2][i] = contact_area[i] * dhat * kappa / (2 * d * d * dhat) * (d + dhat) 34 | else: 35 | IJV[2][i] = 0.0 36 | return IJV 37 | # ANCHOR_END: val_grad_hess 38 | 39 | # ANCHOR: init_step_size 40 | def init_step_size(x, y_ground, p): 41 | alpha = 1 42 | for i in range(0, len(x)): 43 | if p[i][1] < 0: 44 | alpha = min(alpha, 0.9 * (y_ground - x[i][1]) / p[i][1]) 45 | return alpha 46 | # ANCHOR_END: init_step_size -------------------------------------------------------------------------------- /9_reduced_DOF/GravityEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | gravity = [0.0, -9.81] 4 | 5 | def val(x, m): 6 | sum = 0.0 7 | for i in range(0, len(x)): 8 | sum += -m[i] * x[i].dot(gravity) 9 | return sum 10 | 11 | def grad(x, m): 12 | g = np.array([gravity] * len(x)) 13 | for i in range(0, len(x)): 14 | g[i] *= -m[i] 15 | return g 16 | 17 | # Hessian is 0 -------------------------------------------------------------------------------- /9_reduced_DOF/InertiaEnergy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def val(x, x_tilde, m): 4 | sum = 0.0 5 | for i in range(0, len(x)): 6 | diff = x[i] - x_tilde[i] 7 | sum += 0.5 * m[i] * diff.dot(diff) 8 | return sum 9 | 10 | def grad(x, x_tilde, m): 11 | g = np.array([[0.0, 0.0]] * len(x)) 12 | for i in range(0, len(x)): 13 | g[i] = m[i] * (x[i] - x_tilde[i]) 14 | return g 15 | 16 | def hess(x, x_tilde, m): 17 | IJV = [[0] * (len(x) * 2), [0] * (len(x) * 2), np.array([0.0] * (len(x) * 2))] 18 | for i in range(0, len(x)): 19 | for d in range(0, 2): 20 | IJV[0][i * 2 + d] = i * 2 + d 21 | IJV[1][i * 2 + d] = i * 2 + d 22 | IJV[2][i * 2 + d] = m[i] 23 | return IJV -------------------------------------------------------------------------------- /9_reduced_DOF/readme.md: -------------------------------------------------------------------------------- 1 | # Reduced Simulation of Neo-Hookean Solids 2 | 3 | A square falling onto the ground under gravity is simulated using neo-Hookean elasticity and implicit Euler time integration in the reduced solution space constructed via polynomial functions or modal-order reduction. 4 | Each time step is solved by minimizing the Incremental Potential with the projected Newton method. 5 | 6 | ![results](results.gif) 7 | 8 | ## Dependencies 9 | ``` 10 | pip install numpy scipy pygame 11 | ``` 12 | 13 | ## Run 14 | ``` 15 | python simulator.py 16 | ``` -------------------------------------------------------------------------------- /9_reduced_DOF/results.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/phys-sim-book/solid-sim-tutorial/f9d4811b2f95bdf7c189669fb9b614b8baab6fcf/9_reduced_DOF/results.gif -------------------------------------------------------------------------------- /9_reduced_DOF/simulator.py: -------------------------------------------------------------------------------- 1 | # Mass-Spring Solids Simulation 2 | 3 | import numpy as np # numpy for linear algebra 4 | import pygame # pygame for visualization 5 | pygame.init() 6 | 7 | import utils 8 | import square_mesh # square mesh 9 | import time_integrator 10 | 11 | # simulation setup 12 | side_len = 1 13 | rho = 1000 # density of square 14 | E = 2e4 # Young's modulus 15 | nu = 0.4 # Poisson's ratio 16 | n_seg = 10 # num of segments per side of the square 17 | h = 0.01 # time step size in s 18 | DBC = [] # no nodes need to be fixed 19 | y_ground = -1 # height of the planar ground 20 | 21 | # initialize simulation 22 | [x, e] = square_mesh.generate(side_len, n_seg) # node positions and edge node indices 23 | v = np.array([[0.0, 0.0]] * len(x)) # velocity 24 | m = [rho * side_len * side_len / ((n_seg + 1) * (n_seg + 1))] * len(x) # calculate node mass evenly 25 | # ANCHOR: elem_precomp 26 | # rest shape basis, volume, and lame parameters 27 | vol = [0.0] * len(e) 28 | IB = [np.array([[0.0, 0.0]] * 2)] * len(e) 29 | for i in range(0, len(e)): 30 | TB = [x[e[i][1]] - x[e[i][0]], x[e[i][2]] - x[e[i][0]]] 31 | vol[i] = np.linalg.det(np.transpose(TB)) / 2 32 | IB[i] = np.linalg.inv(np.transpose(TB)) 33 | mu_lame = [0.5 * E / (1 + nu)] * len(e) 34 | lam = [E * nu / ((1 + nu) * (1 - 2 * nu))] * len(e) 35 | # ANCHOR_END: elem_precomp 36 | # identify whether a node is Dirichlet 37 | is_DBC = [False] * len(x) 38 | for i in DBC: 39 | is_DBC[i] = True 40 | # ANCHOR: contact_area 41 | contact_area = [side_len / n_seg] * len(x) # perimeter split to each node 42 | # ANCHOR_END: contact_area 43 | # compute reduced basis using 0: no reduction; 1: polynomial functions; 2: modal reduction 44 | reduced_basis = utils.compute_reduced_basis(x, e, vol, IB, mu_lame, lam, method=1, order=2) 45 | 46 | # simulation with visualization 47 | resolution = np.array([900, 900]) 48 | offset = resolution / 2 49 | scale = 200 50 | def screen_projection(x): 51 | return [offset[0] + scale * x[0], resolution[1] - (offset[1] + scale * x[1])] 52 | 53 | time_step = 0 54 | square_mesh.write_to_file(time_step, x, e) 55 | screen = pygame.display.set_mode(resolution) 56 | running = True 57 | while running: 58 | # run until the user asks to quit 59 | for event in pygame.event.get(): 60 | if event.type == pygame.QUIT: 61 | running = False 62 | 63 | print('### Time step', time_step, '###') 64 | 65 | # fill the background and draw the square 66 | screen.fill((255, 255, 255)) 67 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection([-2, y_ground]), screen_projection([2, y_ground])) # ground 68 | for eI in e: 69 | # ANCHOR: draw_tri 70 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[0]]), screen_projection(x[eI[1]])) 71 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[1]]), screen_projection(x[eI[2]])) 72 | pygame.draw.aaline(screen, (0, 0, 255), screen_projection(x[eI[2]]), screen_projection(x[eI[0]])) 73 | # ANCHOR_END: draw_tri 74 | for xI in x: 75 | pygame.draw.circle(screen, (0, 0, 255), screen_projection(xI), 0.1 * side_len / n_seg * scale) 76 | 77 | pygame.display.flip() # flip the display 78 | 79 | # step forward simulation and wait for screen refresh 80 | [x, v] = time_integrator.step_forward(x, e, v, m, vol, IB, mu_lame, lam, y_ground, contact_area, is_DBC, reduced_basis, h, 1e-2) 81 | time_step += 1 82 | pygame.time.wait(int(h * 1000)) 83 | square_mesh.write_to_file(time_step, x, e) 84 | 85 | pygame.quit() -------------------------------------------------------------------------------- /9_reduced_DOF/square_mesh.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | def generate(side_length, n_seg): 5 | # sample nodes uniformly on a square 6 | x = np.array([[0.0, 0.0]] * ((n_seg + 1) ** 2)) 7 | step = side_length / n_seg 8 | for i in range(0, n_seg + 1): 9 | for j in range(0, n_seg + 1): 10 | x[i * (n_seg + 1) + j] = [-side_length / 2 + i * step, -side_length / 2 + j * step] 11 | 12 | # ANCHOR: tri_vert_ind 13 | # connect the nodes with triangle elements 14 | e = [] 15 | for i in range(0, n_seg): 16 | for j in range(0, n_seg): 17 | # triangulate each cell following a symmetric pattern: 18 | if (i % 2)^(j % 2): 19 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, i * (n_seg + 1) + j + 1]) 20 | e.append([(i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 21 | else: 22 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1]) 23 | e.append([i * (n_seg + 1) + j, (i + 1) * (n_seg + 1) + j + 1, i * (n_seg + 1) + j + 1]) 24 | # ANCHOR_END: tri_vert_ind 25 | 26 | return [x, e] 27 | 28 | def write_to_file(frameNum, x, e): 29 | # Check if 'output' directory exists; if not, create it 30 | if not os.path.exists('output'): 31 | os.makedirs('output') 32 | 33 | # create obj file 34 | filename = f"output/{frameNum}.obj" 35 | with open(filename, 'w') as f: 36 | # write vertex coordinates 37 | for row in x: 38 | f.write(f"v {float(row[0]):.6f} {float(row[1]):.6f} 0.0\n") 39 | # write vertex indices for each triangle 40 | for row in e: 41 | #NOTE: vertex indices start from 1 in obj file format 42 | f.write(f"f {row[0] + 1} {row[1] + 1} {row[2] + 1}\n") -------------------------------------------------------------------------------- /9_reduced_DOF/time_integrator.py: -------------------------------------------------------------------------------- 1 | import copy 2 | from cmath import inf 3 | 4 | import numpy as np 5 | import numpy.linalg as LA 6 | import scipy.sparse as sparse 7 | from scipy.sparse.linalg import spsolve 8 | 9 | import InertiaEnergy 10 | import NeoHookeanEnergy 11 | import GravityEnergy 12 | import BarrierEnergy 13 | 14 | def step_forward(x, e, v, m, vol, IB, mu_lame, lam, y_ground, contact_area, is_DBC, reduced_basis, h, tol): 15 | x_tilde = x + v * h # implicit Euler predictive position 16 | x_n = copy.deepcopy(x) 17 | 18 | # Newton loop 19 | iter = 0 20 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h) 21 | p = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, is_DBC, reduced_basis, h) 22 | while LA.norm(p, inf) / h > tol: 23 | print('Iteration', iter, ':') 24 | print('residual =', LA.norm(p, inf) / h) 25 | 26 | # ANCHOR: filter_ls 27 | # filter line search 28 | alpha = BarrierEnergy.init_step_size(x, y_ground, p) # avoid interpenetration and tunneling 29 | while IP_val(x + alpha * p, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h) > E_last: 30 | alpha /= 2 31 | # ANCHOR_END: filter_ls 32 | print('step size =', alpha) 33 | 34 | x += alpha * p 35 | E_last = IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h) 36 | p = search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, is_DBC, reduced_basis, h) 37 | iter += 1 38 | 39 | v = (x - x_n) / h # implicit Euler velocity update 40 | return [x, v] 41 | 42 | def IP_val(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h): 43 | return InertiaEnergy.val(x, x_tilde, m) + h * h * (NeoHookeanEnergy.val(x, e, vol, IB, mu_lame, lam) + GravityEnergy.val(x, m) + BarrierEnergy.val(x, y_ground, contact_area)) # implicit Euler 44 | 45 | def IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h): 46 | return InertiaEnergy.grad(x, x_tilde, m) + h * h * (NeoHookeanEnergy.grad(x, e, vol, IB, mu_lame, lam) + GravityEnergy.grad(x, m) + BarrierEnergy.grad(x, y_ground, contact_area)) # implicit Euler 47 | 48 | def IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h): 49 | IJV_In = InertiaEnergy.hess(x, x_tilde, m) 50 | IJV_MS = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam) 51 | IJV_B = BarrierEnergy.hess(x, y_ground, contact_area) 52 | IJV_MS[2] *= h * h # implicit Euler 53 | IJV_B[2] *= h * h # implicit Euler 54 | IJV_In_MS = np.append(IJV_In, IJV_MS, axis=1) 55 | IJV = np.append(IJV_In_MS, IJV_B, axis=1) 56 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 57 | return H 58 | 59 | def search_dir(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, is_DBC, reduced_basis, h): 60 | projected_hess = IP_hess(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h) 61 | reshaped_grad = IP_grad(x, e, x_tilde, m, vol, IB, mu_lame, lam, y_ground, contact_area, h).reshape(len(x) * 2, 1) 62 | # eliminate DOF by modifying gradient and Hessian for DBC: 63 | for i, j in zip(*projected_hess.nonzero()): 64 | if is_DBC[int(i / 2)] | is_DBC[int(j / 2)]: 65 | projected_hess[i, j] = (i == j) 66 | for i in range(0, len(x)): 67 | if is_DBC[i]: 68 | reshaped_grad[i * 2] = reshaped_grad[i * 2 + 1] = 0.0 69 | reduced_hess = reduced_basis.T.dot(projected_hess.dot(reduced_basis)) # applying chain rule 70 | reduced_grad = reduced_basis.T.dot(reshaped_grad) # applying chain rule 71 | return (reduced_basis.dot(spsolve(reduced_hess, -reduced_grad))).reshape(len(x), 2) # transform to full space after the solve -------------------------------------------------------------------------------- /9_reduced_DOF/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as LA 3 | 4 | import scipy.sparse as sparse 5 | from scipy.sparse.linalg import eigsh 6 | 7 | import NeoHookeanEnergy 8 | 9 | def make_PSD(hess): 10 | [lam, V] = LA.eigh(hess) # Eigen decomposition on symmetric matrix 11 | # set all negative Eigenvalues to 0 12 | for i in range(0, len(lam)): 13 | lam[i] = max(0, lam[i]) 14 | return np.matmul(np.matmul(V, np.diag(lam)), np.transpose(V)) 15 | 16 | # ANCHOR: find_positive_real_root 17 | def smallest_positive_real_root_quad(a, b, c, tol = 1e-6): 18 | # return negative value if no positive real root is found 19 | t = 0 20 | if abs(a) <= tol: 21 | if abs(b) <= tol: # f(x) = c > 0 for all x 22 | t = -1 23 | else: 24 | t = -c / b 25 | else: 26 | desc = b * b - 4 * a * c 27 | if desc > 0: 28 | t = (-b - math.sqrt(desc)) / (2 * a) 29 | if t < 0: 30 | t = (-b + math.sqrt(desc)) / (2 * a) 31 | else: # desv<0 ==> imag, f(x) > 0 for all x > 0 32 | t = -1 33 | return t 34 | # ANCHOR_END: find_positive_real_root 35 | 36 | def compute_reduced_basis(x, e, vol, IB, mu_lame, lam, method, order): 37 | if method == 0: # full basis, no reduction 38 | basis = np.zeros((len(x) * 2, len(x) * 2)) 39 | for i in range(len(x) * 2): 40 | basis[i][i] = 1 41 | return basis 42 | elif method == 1: # polynomial basis 43 | if order == 1: # linear basis, or affine basis 44 | basis = np.zeros((len(x) * 2, 6)) # 1, x, y for both x- and y-displacements 45 | for i in range(len(x)): 46 | for d in range(2): 47 | basis[i * 2 + d][d * 3] = 1 48 | basis[i * 2 + d][d * 3 + 1] = x[i][0] 49 | basis[i * 2 + d][d * 3 + 2] = x[i][1] 50 | elif order == 2: # quadratic polynomial basis 51 | basis = np.zeros((len(x) * 2, 12)) # 1, x, y, x^2, xy, y^2 for both x- and y-displacements 52 | for i in range(len(x)): 53 | for d in range(2): 54 | basis[i * 2 + d][d * 6] = 1 55 | basis[i * 2 + d][d * 6 + 1] = x[i][0] 56 | basis[i * 2 + d][d * 6 + 2] = x[i][1] 57 | basis[i * 2 + d][d * 6 + 3] = x[i][0] * x[i][0] 58 | basis[i * 2 + d][d * 6 + 4] = x[i][0] * x[i][1] 59 | basis[i * 2 + d][d * 6 + 5] = x[i][1] * x[i][1] 60 | elif order == 3: # cubic polynomial basis 61 | basis = np.zeros((len(x) * 2, 20)) # 1, x, y, x^2, xy, y^2, x^3, x^2y, xy^2, y^3 for both x- and y-displacements 62 | for i in range(len(x)): 63 | for d in range(2): 64 | basis[i * 2 + d][d * 10] = 1 65 | basis[i * 2 + d][d * 10 + 1] = x[i][0] 66 | basis[i * 2 + d][d * 10 + 2] = x[i][1] 67 | basis[i * 2 + d][d * 10 + 3] = x[i][0] * x[i][0] 68 | basis[i * 2 + d][d * 10 + 4] = x[i][0] * x[i][1] 69 | basis[i * 2 + d][d * 10 + 5] = x[i][1] * x[i][1] 70 | basis[i * 2 + d][d * 10 + 6] = x[i][0] * x[i][0] * x[i][0] 71 | basis[i * 2 + d][d * 10 + 7] = x[i][0] * x[i][0] * x[i][1] 72 | basis[i * 2 + d][d * 10 + 8] = x[i][0] * x[i][1] * x[i][1] 73 | basis[i * 2 + d][d * 10 + 9] = x[i][1] * x[i][1] * x[i][1] 74 | else: 75 | print("unsupported order of polynomial basis for reduced DOF") 76 | exit() 77 | return basis 78 | else: # modal-order reduction 79 | if order <= 0 or order >= len(x) * 2: 80 | print("invalid number of target basis for modal reduction") 81 | exit() 82 | IJV = NeoHookeanEnergy.hess(x, e, vol, IB, mu_lame, lam, project_PSD=False) 83 | H = sparse.coo_matrix((IJV[2], (IJV[0], IJV[1])), shape=(len(x) * 2, len(x) * 2)).tocsr() 84 | eigenvalues, eigenvectors = eigsh(H, k=order, which='SM') # get 'order' eigenvectors with smallest eigenvalues 85 | return eigenvectors -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Solid Simulation Tutorials 2 | 3 | A curated collection of Python examples focused on optimization-based solid simulation with guarantees on algorithmic convergence, penetration-free and inversion-free conditions. The examples are designed for ease of readability and understanding. 4 | 5 | Introductory sections (with * following the titles) for each of the examples in this repository can be found in the free online book [Physics-based Simulation](https://phys-sim-book.github.io/). 6 | 7 | A [MUDA](https://github.com/MuGdxy/muda)-based GPU version of the tutorial written by Zhaofeng Luo ([@Roushelfy](https://github.com/Roushelfy)) can be found at [solid-sim-tutorial-gpu](https://github.com/phys-sim-book/solid-sim-tutorial-gpu). 8 | --------------------------------------------------------------------------------