(right click and select open image in New tab if not loading)

-- Rigid Body Physics --
print("Added rigid body physics element: RBOD.")

local rbod = elem.allocate("USER", "RBOD")
elem.element(rbod, elem.element(elem.DEFAULT_PT_BCOL))
elem.property(rbod, "Name", "RBOD")
elem.property(rbod, "Description", "Rigid Body. Forms a single body with other RBOD particles.")
elem.property(rbod, "Color", 0xCCCCCC)
elem.property(rbod, "Properties", elem.TYPE_SOLID)
elem.property(rbod, "Weight", 50)

local bodies = {}
local body_id_counter = 1
local is_drawing_rbod = false

local function get_body_id(i)
return sim.partProperty(i, "tmp")
end

local function set_body_id(i, id)
sim.partProperty(i, "tmp", id)
end

local function get_body(id)
if not bodies[id] then
bodies[id] = { particles = {}, rel_positions = {}, center_x = 0, center_y = 0, vx = 0, vy = 0, angle = 0, av = 0, mass = 0, moment_of_inertia = 0 }
end
return bodies[id]
end

local function update_body_properties(id)
local body = get_body(id)
if not body or #body.particles == 0 then
return
end

local total_x, total_y = 0, 0
for _, p_id in ipairs(body.particles) do
local x, y = sim.partPosition(p_id)
if x and y then
total_x = total_x + x
total_y = total_y + y
end
end

if #body.particles > 0 then
body.center_x = total_x / #body.particles
body.center_y = total_y / #body.particles
body.mass = #body.particles * 50

-- Calculate moment of inertia and radius for broad-phase collision
local max_dist_sq = 0
body.moment_of_inertia = 0
local particle_mass = 50
for _, p_id in ipairs(body.particles) do
local x, y = sim.partPosition(p_id)
if x and y then
local dist_sq = (x - body.center_x)^2 + (y - body.center_y)^2
if dist_sq > max_dist_sq then
max_dist_sq = dist_sq
end
body.moment_of_inertia = body.moment_of_inertia + particle_mass * dist_sq
end
end
body.radius = math.sqrt(max_dist_sq)
end
end

local function rbod_update(i, x, y, s, nt)
local body_id = get_body_id(i)
if body_id == 0 then
-- Flood fill to find connected particles
local new_body_id = body_id_counter
body_id_counter = body_id_counter + 1
local q = {i}
local visited = {[i] = true}
set_body_id(i, new_body_id)
local body = get_body(new_body_id)
table.insert(body.particles, i)

local head = 1
while head <= #q do
local current_part = q[head]
head = head + 1

local cx, cy = sim.partPosition(current_part)
if cx and cy then
for nx = cx - 1, cx + 1 do
for ny = cy - 1, cy + 1 do
if not (nx == cx and ny == cy) then
local r = sim.partID(nx, ny)
if r and not visited[r] and sim.partProperty(r, "type") == rbod and get_body_id(r) == 0 then
visited[r] = true
set_body_id(r, new_body_id)
table.insert(body.particles, r)
table.insert(q, r)
end
end
end
end
end
end

update_body_properties(new_body_id)

-- Store relative positions
body.rel_positions = {}
for _, p_id in ipairs(body.particles) do
local px, py = sim.partPosition(p_id)
if px and py then
table.insert(body.rel_positions, {x = px - body.center_x, y = py - body.center_y})
end
end
end
end

elem.property(rbod, "Update", rbod_update)

local function move_bodies()
if is_drawing_rbod then return end -- Don't apply physics while drawing

-- Cleanup dead particles and empty bodies first
local dead_bodies = {}
for id, body in pairs(bodies) do
local living_particles = {}
local living_rel_positions = {}
local has_changed = false
for i, p_id in ipairs(body.particles) do
if sim.partPosition(p_id) then
table.insert(living_particles, p_id)
table.insert(living_rel_positions, body.rel_positions[i])
else
has_changed = true
end
end

if #living_particles == 0 then
table.insert(dead_bodies, id)
elseif has_changed then
body.particles = living_particles
body.rel_positions = living_rel_positions
update_body_properties(id)
end
end

for _, id in ipairs(dead_bodies) do
bodies[id] = nil
end

-- Physics and move phase
local body_ids = {}
for id, _ in pairs(bodies) do
table.insert(body_ids, id)
end

for i = 1, #body_ids do
local id1 = body_ids[i]
local body1 = bodies[id1]
if body1 then
-- Apply gravity
body1.vy = body1.vy + 0.1

-- Collision with other bodies
for j = i + 1, #body_ids do
local id2 = body_ids[j]
local body2 = bodies[id2]
if body2 then
local dx = body2.center_x - body1.center_x
local dy = body2.center_y - body1.center_y
local dist_sq = dx*dx + dy*dy
local radius1 = body1.radius or 0
local radius2 = body2.radius or 0
if dist_sq < (radius1 + radius2)^2 and dist_sq > 0 then
-- Narrow-phase: check for particle-to-particle collision
local collision_info = nil
local collision_found = false
for _, p1_id in ipairs(body1.particles) do
local x1, y1 = sim.partPosition(p1_id)
if x1 and y1 then
for _, p2_id in ipairs(body2.particles) do
local x2, y2 = sim.partPosition(p2_id)
if x2 and y2 then
local pdx, pdy = x2 - x1, y2 - y1
local p_dist_sq = pdx*pdx + pdy*pdy
if p_dist_sq < 4.0 and p_dist_sq > 0 then -- particles closer than 2 pixels
local p_dist = math.sqrt(p_dist_sq)
collision_info = {
nx = pdx / p_dist, ny = pdy / p_dist,
overlap = 2.0 - p_dist,
p1_x = x1, p1_y = y1,
p2_x = x2, p2_y = y2
}
collision_found = true
break
end
end
end
end
if collision_found then
break
end
end

if collision_info then
-- Resolve overlap
local mtdx = collision_info.nx * collision_info.overlap
local mtdy = collision_info.ny * collision_info.overlap

local inv_mass1 = 1 / body1.mass
local inv_mass2 = 1 / body2.mass

body1.center_x = body1.center_x - mtdx * (inv_mass1 / (inv_mass1 + inv_mass2))
body1.center_y = body1.center_y - mtdy * (inv_mass1 / (inv_mass1 + inv_mass2))
body2.center_x = body2.center_x + mtdx * (inv_mass2 / (inv_mass1 + inv_mass2))
body2.center_y = body2.center_y + mtdy * (inv_mass2 / (inv_mass1 + inv_mass2))

-- Impulse-based collision response with rotation
local nx = collision_info.nx
local ny = collision_info.ny

local r1_x = collision_info.p1_x - body1.center_x
local r1_y = collision_info.p1_y - body1.center_y
local r2_x = collision_info.p2_x - body2.center_x
local r2_y = collision_info.p2_y - body2.center_y

local r1_perp_x, r1_perp_y = -r1_y, r1_x
local r2_perp_x, r2_perp_y = -r2_y, r2_x

local v1_x = body1.vx + body1.av * r1_perp_x
local v1_y = body1.vy + body1.av * r1_perp_y
local v2_x = body2.vx + body2.av * r2_perp_x
local v2_y = body2.vy + body2.av * r2_perp_y

local rvx = v2_x - v1_x
local rvy = v2_y - v1_y

local vel_along_normal = rvx * nx + rvy * ny

if vel_along_normal < 0 then
local e = 0.4 -- restitution

local r1_cross_n = r1_x * ny - r1_y * nx
local r2_cross_n = r2_x * ny - r2_y * nx

local inv_moi1 = body1.moment_of_inertia > 0 and 1 / body1.moment_of_inertia or 0
local inv_moi2 = body2.moment_of_inertia > 0 and 1 / body2.moment_of_inertia or 0

local j = -(1 + e) * vel_along_normal
j = j / (inv_mass1 + inv_mass2 + (r1_cross_n^2) * inv_moi1 + (r2_cross_n^2) * inv_moi2)

local impulse_x = j * nx
local impulse_y = j * ny

body1.vx = body1.vx - inv_mass1 * impulse_x
body1.vy = body1.vy - inv_mass1 * impulse_y
body1.av = body1.av - (r1_x * impulse_y - r1_y * impulse_x) * inv_moi1

body2.vx = body2.vx + inv_mass2 * impulse_x
body2.vy = body2.vy + inv_mass2 * impulse_y
body2.av = body2.av + (r2_x * impulse_y - r2_y * impulse_x) * inv_moi2
end
end
end
end
end

-- Move body center
body1.center_x = body1.center_x + body1.vx
body1.center_y = body1.center_y + body1.vy
body1.angle = body1.angle + body1.av

-- Wall collision for the whole body
local radius = body1.radius or 0
if body1.center_x - radius < 0 then body1.center_x = radius; body1.vx = -body1.vx * 0.8 end
if body1.center_x + radius >= sim.XRES then body1.center_x = sim.XRES - 1 - radius; body1.vx = -body1.vx * 0.8 end
if body1.center_y - radius < 0 then body1.center_y = radius; body1.vy = -body1.vy * 0.8 end
if body1.center_y + radius >= sim.YRES then body1.center_y = sim.YRES - 1 - radius; body1.vy = -body1.vy * 0.8 end

-- Move particles based on new body center and angle
local cos_a = math.cos(body1.angle)
local sin_a = math.sin(body1.angle)
for k, p_id in ipairs(body1.particles) do
local rel_pos = body1.rel_positions[k]
if rel_pos then
local new_x = body1.center_x + rel_pos.x * cos_a - rel_pos.y * sin_a
local new_y = body1.center_y + rel_pos.x * sin_a + rel_pos.y * cos_a
sim.partProperty(p_id, "x", math.floor(new_x + 0.5))
sim.partProperty(p_id, "y", math.floor(new_y + 0.5))
end
end

-- Dampening
body1.vx = body1.vx * 0.98
body1.vy = body1.vy * 0.98
body1.av = body1.av * 0.98
end
end
end

local function rbod_mouse_click(mousex, mousey, button, event)
local selected_element = tpt.selectedl
if button == 4 then selected_element = tpt.selectedr end

if selected_element == rbod then
if event == 1 then -- Mouse down
is_drawing_rbod = true
elseif event == 2 then -- Mouse up
is_drawing_rbod = false
end
end
return true
end

tpt.register_step(move_bodies)
tpt.register_mouseclick(rbod_mouse_click)