Skip to content

Instantly share code, notes, and snippets.

@xenobrain

xenobrain/gjk.rb

Created April 19, 2023 18:03
Show Gist options
  • Save xenobrain/46356970992a3dd2fcb5a776ca60fe8b to your computer and use it in GitHub Desktop.
Save xenobrain/46356970992a3dd2fcb5a776ca60fe8b to your computer and use it in GitHub Desktop.
Download ZIP
Physics WIP
Raw
gjk.rb
DEG2RAD = Math::PI / 180.0
def tick args
b = args.state.box_a ||= { x: 540, y: 360, w: 200, h: 200, angle: 45, path: 'sprites/square/blue.png' }
a = args.state.box_b ||= { x: 340, y: 360, w: 100, h: 100, angle: 45, path: 'sprites/square/green.png' }
$a = a
a.y += 1 if args.inputs.up
a.y -= 1 if args.inputs.down
a.x -= 1 if args.inputs.left
a.x += 1 if args.inputs.right
a.angle -= 1 if args.inputs.keyboard.e
a.angle += 1 if args.inputs.keyboard.q
args.outputs.background_color = [32, 32, 32]
args.outputs.sprites << b << a
start = Time.now
100.times do
p = closest_points a, b
#p = find_collision_polygon_polygon a, b
end
stop = (Time.now - start) * 1000
#if p
# args.outputs.sprites << p.contacts.map do |c|
# args.outputs.sprites << { x: c.r1x - 4, y: c.r1y - 4, w: 8, h: 8, path: 'sprites/circle/yellow.png' }
# args.outputs.sprites << { x: c.r2x - 4, y: c.r2y - 4, w: 8, h: 8, path: 'sprites/circle/orange.png' }
# end
#end
#a.intersect_rect? b
#args.outputs.labels << { x: 10, y: 500, text: "#{stop}", r: 255, g: 255, b: 255 }
#puts stop
end
def furthest p, dx, dy
p_len = p.length
index = 0
x, y = p[0]
max = x * dx + y * dy
i = 1
while i < p_len
x, y = p[i]
d = x * dx + y * dy
if d > max
max = d
index = i
end
i += 1
end
index
end
def closest_points a, b
dx = a.x - b.x
dy = a.y - b.y
a = create_vertices a
b = create_vertices b
## Initialize the Simplex ##
# Point A
index_a = furthest a, -dx, -dy
index_b = furthest b, dx, dy
a_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
ax, ay = a[index_a]
bx, by = b[index_b]
simplex_a = [bx - ax, by - ay, index_a, index_b, a_id]
# Point B
index_a = furthest a, dx, dy
index_b = furthest b, -dx, -dy
b_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
ax, ay = a[index_a]
bx, by = b[index_b]
simplex_b = [bx - ax, by - ay, index_a, index_b, b_id]
## Iterations ##
while true
simplex_ax, simplex_ay, a_index_a, a_index_b, a_id = simplex_a
simplex_bx, simplex_by, b_index_a, b_index_b, b_id = simplex_b
# Preserve winding order by flipping the points
if (simplex_by - simplex_ay) * (simplex_ax + simplex_bx) < (simplex_bx - simplex_ax) * (simplex_ay + simplex_by)
t = simplex_a
simplex_a = simplex_b
simplex_b = t
next
end
# Find closest point on the line to the origin
line_x = simplex_bx - simplex_ax
line_y = simplex_by - simplex_ay
closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
(line_x * line_x + line_y * line_y)).clamp(-1, 1)
# If the origin projection is in Voronoi region AB, the search direction is perpendicular to the simplex
if closest_t > -1 && closest_t < 1
dy = (simplex_bx - simplex_ax)
dx = -(simplex_by - simplex_ay)
else
# The point is in Voronoi region A or B, lerp along the simplex towards the origin
closest_t *= 0.5
dx = -(simplex_ax * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t))
dy = -(simplex_ay * (0.5 - closest_t) + simplex_by * (0.5 + closest_t))
end
# Point C
index_a = furthest a, -dx, -dy
index_b = furthest b, dx, dy
id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
break if id == a_id || id == b_id || index_a == a_index_a || index_a == b_index_a || index_b == a_index_b || index_b == b_index_b
ax, ay = a[index_a]
bx, by = b[index_b]
simplex_c = [bx - ax, by - ay, index_a, index_b, id]
simplex_cx, simplex_cy = simplex_c
# If CA and BC are to the right of the origin, the simplex contains the origin
if ((simplex_ay - simplex_cy) * (simplex_cx + simplex_ax) > (simplex_ax - simplex_cx) * (simplex_cy + simplex_ay)) &&
((simplex_cy - simplex_by) * (simplex_bx + simplex_cx) > (simplex_cx - simplex_bx) * (simplex_by + simplex_cy))
$gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
break
end
# Simplex A and B are closer to the origin than Simplex C
na = simplex_ax * dx + simplex_ay * dy
nb = simplex_bx * dx + simplex_by * dy
break if (simplex_cx * dx + simplex_cy * dy) <= (na > nb ? na : nb)
## Drop the furthest point from the origin ##
# distance from origin A, C
line_x = simplex_cx - simplex_ax
line_y = simplex_cy - simplex_ay
closest_t = -((line_x * (simplex_ax + simplex_cx) + line_y * (simplex_ay + simplex_cy)) /
(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
x = simplex_ax * (0.5 - closest_t) + simplex_cx * (0.5 + closest_t)
y = simplex_ay * (0.5 - closest_t) + simplex_cy * (0.5 + closest_t)
ac = x * x + y * y
# distance from origin C, B
line_x = simplex_bx - simplex_cx
line_y = simplex_by - simplex_cy
closest_t = -((line_x * (simplex_cx + simplex_bx) + line_y * (simplex_cy + simplex_by)) /
(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
x = simplex_cx * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t)
y = simplex_cy * (0.5 - closest_t) + simplex_by * (0.5 + closest_t)
cb = x * x + y * y
ac < cb ? simplex_b = simplex_c : simplex_a = simplex_c
end
a_ax, a_ay, a_index_a, a_index_b, id = simplex_a
b_ax, b_ay, b_index_a, b_index_b, id = simplex_b
# closest point on the simplex
line_x = simplex_bx - simplex_ax
line_y = simplex_by - simplex_ay
closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
#closest_sx = a_ax * (0.5 - closest_t) + a_bx * (0.5 + closest_t)
#closest_sy = a_ay * (0.5 - closest_t) + a_by * (0.5 + closest_t)
# interpolate original points to get the closest points
a_ax, a_ay = a[a_index_a]
b_ax, b_ay = a[b_index_a]
closest_ax = a_ax * (0.5 - closest_t) + b_ax * (0.5 + closest_t)
closest_ay = a_ay * (0.5 - closest_t) + b_ay * (0.5 + closest_t)
a_bx, a_by = b[a_index_b]
b_bx, b_by = b[b_index_b]
closest_bx = a_bx * (0.5 - closest_t) + b_bx * (0.5 + closest_t)
closest_by = a_by * (0.5 - closest_t) + b_by * (0.5 + closest_t)
$gtk.args.outputs.lines << { x: closest_ax, y: closest_ay, x2: closest_bx, y2: closest_by, g: 255 }
end
def create_vertices rect
x = rect.x; y = rect.y
w = rect.w; h = rect.h
cx = x + w * 0.5; cy = y + h * 0.5
sin = Math.sin (rect.angle || 0.0) * DEG2RAD; cos = Math.cos (rect.angle || 0.0) * DEG2RAD
[
[(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy], # Bottom Left
[(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy], # Bottom Right
[(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
[(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy] # Top Left
].reverse
end
def find_collision_polygon_polygon a, b
a = create_vertices a
b = create_vertices b
## Find Minimum Separation and Reference Edge ##
separation = -Float::INFINITY
k = 0
while k < 2
a_length = a.length
b_length = b.length
i = 0
while i < b_length
v1x, v1y = b[(i + 1) % b_length]
v0x, v0y = b[i]
nx = -(v1y - v0y)
ny = v1x - v0x
l = 1.0 / (Math.sqrt nx * nx + ny * ny)
nx *= l
ny *= l
v2x, v2y = a[0]
min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny
j = 1
while j < a_length
v2x, v2y = a[j]
projection = (v2x - v0x) * nx + (v2y - v0y) * ny
min_projection = projection if projection < min_projection
j += 1
end
# early exit if the axis are separating
return if min_projection >= 0
# bias to improve coherence
if min_projection * 0.95 > separation + 0.01
separation = min_projection
reference_edge_ax = v0x
reference_edge_ay = v0y
reference_edge_bx = v1x
reference_edge_by = v1y
incident_shape = a
normal_x = nx
normal_y = ny
end
i += 1
end
# swap shapes for the next test
t = a
a = b
b = t
k += 1
end
## Find Incident Edge ##
i_length = incident_shape.length
min_projection = Float::INFINITY
i = 0
while i < i_length
v1x, v1y = incident_shape[(i + 1) % i_length]
v0x, v0y = incident_shape[i]
nx = -(v1y - v0y)
ny = (v1x - v0x)
projection = nx * normal_x + ny * normal_y
if projection < min_projection
min_projection = projection
incident_edge_ax = v0x
incident_edge_ay = v0y
incident_edge_bx = v1x
incident_edge_by = v1y
end
i += 1
end
## Clipping ##
contacts = []
dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x
reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)
# project the segment ends onto the other segment, clamping t
t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)
reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)
if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
contacts << { r1x: reference_point_x,
r1y: reference_point_y,
r2x: incident_point_x,
r2y: incident_point_y,
depth: separation,
jn: 0, jt: 0 }
end
reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia
if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
contacts << { r1x: reference_point_x,
r1y: reference_point_y,
r2x: incident_point_x,
r2y: incident_point_y,
depth: separation,
jn: 0, jt: 0 }
end
{ a: a, ax: 0, ay: 0, b: b, bx: 0, by: 0, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
end
Raw
main.rb
RAD2DEG = 180.0 / Math::PI
DEG2RAD = Math::PI / 180.0
TIME_STEP = 0.2 / 60 # delta time isn't required in DragonRuby but it really handy for tuning and debugging physics
COLLISION_BIAS = 0.05 # adds some energy into the collision to get objects to separate. tune this in steps of 0.01
COLLISION_SLOP = 0.1 # amount shapes are allowed to overlap without triggering correction. helps avoid position jitter
COLLISION_ITERATIONS = 10 # how many times to run the solver. a good range is between 5 and 15
def tick args
args.gtk.reset if args.inputs.keyboard.r
box = args.state.box ||= { x: 300, y: 150, w: 50, h: 50, vx: 0, vy: 0, vw: 0, angle: 0, bounce: 0.8, friction: 0.8, mass: Math::PI*50*50, rotational_inertia: 0.83*(50*50+50*50) * (Math::PI*50*50), path: 'sprites/square/blue.png' }
ground = args.state.ground ||= { x: 100, y: 50, w: 1100, h: 50, vx: 0, vy: 0, vw: 0, bounce: 0.8, friction: 0.8, mass: Float::INFINITY, rotational_inertia: Float::INFINITY, path: 'sprites/square/green.png' }
box.vy -= 100.0 * TIME_STEP
args.outputs.sprites << ground
args.outputs.sprites << box
collision = find_collision_polygon_polygon box, ground
if collision
contact0 = collision.contacts[0]
contact1 = collision.contacts[1]
calc_collision collision
end
box.x += box.vx * TIME_STEP
box.y += box.vy * TIME_STEP
box.angle += box.vw * TIME_STEP * RAD2DEG
end
def find_circle_circle a, b
circle_ar = a.radius || [a.w, a.h].max * 0.5
circle_br = b.radius || [b.w, b.h].max * 0.5
circle_ax = a.x + circle_ar
circle_ay = a.y + circle_ar
circle_bx = b.x + circle_br
circle_by = b.y + circle_br
dx = circle_bx - circle_ax
dy = circle_by - circle_ay
distance = dx * dx + dy * dy
min_distance = circle_ar + circle_br
# distance should be less than the sum of radii
# zero distance means the circles have the same centers, do nothing
return if (distance > min_distance * min_distance) || distance.zero?
distance = Math.sqrt distance
dx /= distance
dy /= distance
contact = { r1x: circle_ax + circle_ar * dx,
r1y: circle_ay + circle_ar * dy,
r2x: circle_bx - circle_br * dx,
r2y: circle_by - circle_br * dy,
depth: distance - min_distance,
jn: 0, jt: 0 }
{ a: a, ax: circle_ax, ay: circle_ay,
b: b, bx: circle_bx, by: circle_by,
normal_x: dx, normal_y: dy,
contacts: [contact] }
end
def find_circle_line c, l
circle_r = c.radius || [c.w, c.h].max * 0.5
line_r = l.radius || 0.0
circle_x = c.x + circle_r
circle_y = c.y + circle_r
line_x = l.x2 - l.x
line_y = l.y2 - l.y
t = ((line_x * (circle_x - l.x) + line_y * (circle_y - l.y)) /
(line_x * line_x + line_y * line_y)).clamp(0.0, 1.0)
closest_x = l.x + line_x * t
closest_y = l.y + line_y * t
dx = closest_x - circle_x
dy = closest_y - circle_y
distance = dx * dx + dy * dy
min_distance = circle_r + line_r
return if (distance > min_distance * min_distance) || distance.zero?
distance = Math.sqrt distance
dx /= distance
dy /= distance
contact = { r1x: circle_x + circle_r * dx,
r1y: circle_y + circle_r * dy,
r2x: closest_x - line_r * dx,
r2y: closest_y - line_r * dy,
depth: distance - min_distance,
jn: 0, jt: 0 }
{ a: c, ax: circle_x, ay: circle_y,
b: l, bx: line_x, by: line_y,
normal_x: dx, normal_y: dy,
contacts: [contact] }
end
def find_collision_polygon_polygon poly_a, poly_b
a_cx = poly_a.x + (poly_a.w/2)
a_cy = poly_a.y + (poly_a.h/2)
b_cx = poly_b.x + (poly_b.w/2)
b_cy = poly_b.y + (poly_b.h/2)
a = create_vertices poly_a
b = create_vertices poly_b
## Find Minimum Separation and Reference Edge ##
separation = -Float::INFINITY
k = 0
while k < 2
a_length = a.length
b_length = b.length
i = 0
while i < b_length
v1x, v1y = b[(i + 1) % b_length]
v0x, v0y = b[i]
nx = -(v1y - v0y)
ny = v1x - v0x
l = 1.0 / (Math.sqrt nx * nx + ny * ny)
nx *= l
ny *= l
v2x, v2y = a[0]
min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny
j = 1
while j < a_length
v2x, v2y = a[j]
projection = (v2x - v0x) * nx + (v2y - v0y) * ny
min_projection = projection if projection < min_projection
j += 1
end
# early exit if the axis are separating
return if min_projection >= 0
# bias to improve coherence
if min_projection * 0.95 > separation + 0.01
separation = min_projection
reference_edge_ax = v0x
reference_edge_ay = v0y
reference_edge_bx = v1x
reference_edge_by = v1y
incident_shape = a
normal_x = nx
normal_y = ny
end
i += 1
end
# swap shapes for the next test
t = a
a = b
b = t
k += 1
end
## Find Incident Edge ##
i_length = incident_shape.length
min_projection = Float::INFINITY
i = 0
while i < i_length
v1x, v1y = incident_shape[(i + 1) % i_length]
v0x, v0y = incident_shape[i]
nx = -(v1y - v0y)
ny = (v1x - v0x)
projection = nx * normal_x + ny * normal_y
if projection < min_projection
min_projection = projection
incident_edge_ax = v0x
incident_edge_ay = v0y
incident_edge_bx = v1x
incident_edge_by = v1y
end
i += 1
end
## Clipping ##
contacts = []
dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x
reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)
# project the segment ends onto the other segment, clamping t
t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)
reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)
if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
contacts << { r1x: reference_point_x,
r1y: reference_point_y,
r2x: incident_point_x,
r2y: incident_point_y,
depth: separation,
jn: 0, jt: 0 }
end
reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia
if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
contacts << { r1x: reference_point_x,
r1y: reference_point_y,
r2x: incident_point_x,
r2y: incident_point_y,
depth: separation,
jn: 0, jt: 0 }
end
{ a: poly_a, ax: a_cx, ay: a_cy, b: poly_b, bx: b_cx, by: b_cy, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
end
def calc_collision collision
return unless collision
a = collision[:a]
b = collision[:b]
nx = collision[:normal_x]
ny = collision[:normal_y]
average_bounce = a.bounce * b.bounce
average_friction = a.friction * b.friction
inv_m_a = 1.0 / a.mass
inv_m_b = 1.0 / b.mass
inv_i_a = 1.0 / a.rotational_inertia
inv_i_b = 1.0 / b.rotational_inertia
inv_mass_sum = inv_m_a + inv_m_b
fn.each collision.contacts do |contact|
# contact point in local space
r1x = contact[:r1x] - collision[:ax]
r1y = contact[:r1y] - collision[:ay]
r2x = contact[:r2x] - collision[:bx]
r2y = contact[:r2y] - collision[:by]
# contact point cross normal, tangent
r1cn = r1x * ny - r1y * nx
r2cn = r2x * ny - r2y * nx
r1ct = r1x * nx + r1y * ny
r2ct = r2x * nx + r2y * ny
# sum of masses in normal and tangent directions
mass_normal = 1.0 / (inv_mass_sum + inv_i_a * r1cn * r1cn + inv_i_b * r2cn * r2cn)
mass_tangent = 1.0 / (inv_mass_sum + inv_i_a * r1ct * r1ct + inv_i_b * r2ct * r2ct)
# penetration correction -- feed positional error into separation impulse (Baumgarte stabilization)
bias = COLLISION_BIAS * [0.0, contact[:depth] + COLLISION_SLOP].min / TIME_STEP
# relative velocity
rvx = b.vx - r2y * b.vw - (a.vx - r1y * a.vw)
rvy = b.vy + r2x * b.vw - (a.vy + r1x * a.vw)
# relative velocity along normal * average bounce
bounce = (rvx * nx + rvy * ny) * average_bounce
COLLISION_ITERATIONS.times do
# update the relative velocity
vrx = b.vx - r2y * b.vw - (a.vx - r1y * a.vw)
vry = b.vy + r2x * b.vw - (a.vy + r1x * a.vw)
# relative velocity along normal and tangent
rvn = vrx * nx + vry * ny
rvt = vrx * -ny + vry * nx
# impulse scalar (aka lambda, lagrange multiplier)
jn = -(bounce + rvn + bias) * mass_normal
previous_jn = contact[:jn]
contact[:jn] = [previous_jn + jn, 0.0].max
# tangent scalar, cannot exceed force along normal (Coulomb's law)
max_jt = average_friction * contact[:jn]
jt = -rvt * mass_tangent
previous_jt = contact[:jt]
contact[:jt] = (previous_jt + jt).clamp(-max_jt, max_jt)
jn = contact[:jn] - previous_jn
jt = contact[:jt] - previous_jt
impulse_x = nx * jn - ny * jt
impulse_y = nx * jt + ny * jn
a[:vx] -= impulse_x * inv_m_a
a[:vy] -= impulse_y * inv_m_a
a[:vw] -= inv_i_a * (r1x * impulse_y - r1y * impulse_x)
b[:vx] += impulse_x * inv_m_b
b[:vy] += impulse_y * inv_m_b
b[:vw] += inv_i_b * (r2x * impulse_y - r2y * impulse_x)
end
end
end
def create_vertices rect
x = rect.x; y = rect.y
w = rect.w; h = rect.h
cx = x + w * 0.5; cy = y + h * 0.5
sin = Math.sin (rect.angle || 0.0) * DEG2RAD; cos = Math.cos (rect.angle || 0.0) * DEG2RAD
[
[(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy], # Bottom Left
[(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy], # Bottom Right
[(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
[(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy] # Top Left
].reverse
end
Raw
quickhull.rb
module Physics
class << self
attr_accessor :debug_draw
INV_3 = 1.0 / 3.0
NUM_ITERATIONS = 10
MAX_GJK_ITERATIONS = 30
MAX_EPA_ITERATIONS = 30
@debug_draw = false
def new_polygon x, y, vertices, density = 1
num_vertices = vertices.length
hull_vertices = []
hull_indices = []
moment_of_inertia = 0
polygon_area = 0
center_x = 0
center_y = 0
right = 0
left = vertices.at(0).at(0)
i = 1
while i < num_vertices
hx, hy = vertices.at(i)
if hx > left
left = hx
right = i
elsif hx == left
right = i if hy < vertices.at(right).at(1)
end
i += 1
end
index = right
while true
next_index = 0
hull_indices << index
hull_index = hull_indices.last
i = 1
while i < num_vertices
if next_index == index
next_index = i
next
end
px, py = vertices.at(i)
hx, hy = vertices.at(hull_index)
nx, ny = vertices.at(next_index)
e1x = nx - hx
e1y = ny - hy
e2x = px - hx
e2y = py - hy
cross = e1x * e2y - e1y * e2x
next_index = i if cross.negative?
next_index = i if cross.zero? && (e2x*e2x + e2y*e2y) > (e1x*e1x + e1y*e1y)
i += 1
end
index = next_index
break if next_index == right
end
num_vertices = hull_indices.length
i = 0
while i < num_vertices
x0, y0 = vertices.at(hull_indices.at(i))
x1, y1 = vertices.at(hull_indices.at((i + 1) % num_vertices))
nx = y1 - y0
ny = x0 - y1
cross = x0*y1 - y0*x1
triangle_area = 0.5*cross
polygon_area += triangle_area
center_x += triangle_area*INV_3*(x0 + x1)
center_y += triangle_area*INV_3*(y0 + y1)
moment_of_inertia += 0.25*INV_3*cross*(x0*x0 + x0*x1 + x1*x1 + y0*y0 + y0*y1 + y1*y1)
hull_vertices << [x0, y0, nx, ny]
i += 1
end
center_x *= 1.0 / polygon_area
center_y *= 1.0 / polygon_area
min_x = min_y = Float::INFINITY
max_x = max_y = -Float::INFINITY
i = 0
while i < num_vertices
vertex = hull_vertices.at i
px = vertex[0] -= center_x - x
py = vertex[1] -= center_y - y
min_x = px if px < min_x
min_y = py if py < min_y
max_x = px if px > max_x
max_y = py if py > max_y
i += 1
end
w = max_x - min_x
h = max_y - min_y
inverse_mass = 1.0 / (density*polygon_area)
inverse_moment_of_inertia = 1.0 / moment_of_inertia
aabb = { x: min_x, y: min_y, w: w, h: h, tick: Kernel.tick_count }
hash = {
x: x,
y: y,
w: w,
h: h,
vertices: hull_vertices,
velocity_x: 0.0,
velocity_y: 0.0,
angular_velocity: 0.0,
aabb: aabb,
radius: [w, h].max * 0.5,
inverse_mass: inverse_mass,
inverse_moment_of_inertia: inverse_moment_of_inertia
}
puts hash.to_s
hash
end
def find_collision_polygon_polygon shape_a, shape_b
## Medium phase ##
radii_sum = shape_a[:radius] + shape_b[:radius]
dx = shape_b[:x] - shape_a[:x]
dy = shape_b[:y] - shape_a[:y]
distance = dx*dx + dy*dy
#return unless distance < radii_sum*radii_sum
current_tick = Kernel.tick_count
if shape_a[:inverse_mass].positive? && shape_a[:aabb][:tick] != current_tick
num_vertices = shape_a[:vertices].length
max_x = max_y = -Float::INFINITY
min_x = min_y = Float::INFINITY
aabb = shape_a[:aabb]
i = 0
while i < num_vertices
x, y = shape_a[:vertices].at i
min_x = x if x < min_x
min_y = y if y < min_y
max_x = x if x > max_x
max_y = y if y > max_y
i += 1
end
aabb[:x] = min_x
aabb[:y] = min_y
aabb[:w] = max_x - min_x
aabb[:h] = max_y - min_y
aabb[:tick] = current_tick
end
if shape_b[:inverse_mass].positive? && shape_b[:aabb][:tick] != current_tick
num_vertices = shape_b[:vertices].length
max_x = max_y = -Float::INFINITY
min_x = min_y = Float::INFINITY
aabb = shape_b[:aabb]
i = 0
while i < num_vertices
x, y = shape_b[:vertices].at i
min_x = x if x < min_x
min_y = y if y < min_y
max_x = x if x > max_x
max_y = y if y > max_y
i += 1
end
aabb[:x] = min_x
aabb[:y] = min_y
aabb[:w] = max_x - min_x
aabb[:h] = max_y - min_y
aabb[:tick] = current_tick
end
#return false unless $gtk.args.geometry.intersect_rect? shape_a[:aabb], shape_b[:aabb]
## Narrow phase ##
# Lookup cached points from the collision ID
# Collision ID is modified from default--for polys it stores the index of 2 minkowski points in a bitfield
support_x = 0
support_y = 0
support_id = 0
support_point = proc do
max_ax = max_bx = max_ay = max_by = max_projection_a = max_projection_b = -Float::INFINITY
index_a = index_b = 0
j = 0
vertices = shape_a[:vertices]
num_vertices = vertices.length
while j < num_vertices
x, y = vertices.at j
projection = x*dx + y*dy
if projection > max_projection_a
max_projection_a = projection
index_a = j
max_ax = x
max_ay = y
end
j += 1
end
j = 0
vertices = shape_b[:vertices]
num_vertices = vertices.length
while j < num_vertices
x, y = vertices.at j
projection = x*dx + y*dy
if projection > max_projection_b
max_projection_b = projection
index_b = j
max_bx = x
max_by = y
end
j += 1
end
support_x = max_ax - max_bx
support_y = max_ay - max_by
support_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
end
# Begin GJK Closest Points algorithm
temp = dx
dx = -dy
dy = temp
support_point.call
point_ax = support_x
point_ay = support_y
dx = -dx
dy = -dy
support_point.call
point_bx = support_x
point_by = support_y
i = 0
while i < MAX_GJK_ITERATIONS
if (point_ay - point_by)*(point_bx + point_ax) > (point_ax - point_bx)*(point_by + point_ay)
temp = point_ax
point_ax = point_bx
point_bx = temp
temp = point_ay
point_by = point_ay
point_ay = temp
next
end
# To find the next search direction, interpolate segment ab towards the origin(0,0) of the simplex
sx = point_bx - point_ax
sy = point_by - point_ay
t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)
if t > -1 && t < 1
# t wasn't clamped, choose a perpendicular direction
dx = -(point_by - point_ay)
dy = point_bx - point_ax
else
# t was clamped, inverse of interpolate point_a, point_b towards the origin with t
t *= 0.5
dx = -((point_ax*(0.5 - t)) + (point_bx*(0.5 + t)))
dy = -((point_ay*(0.5 - t)) + (point_by*(0.5 + t)))
end
# Get a new support point based on the current direction
support_point.call
point_cx = support_x
point_cy = support_y
if @debug_draw
cx = point_ax*0.5 + point_bx*0.5
cy = point_ay*0.5 + point_by*0.5
len = Math.sqrt(dx*dx + dy*dy)
nx = dx*len
ny = dy*len
cx2 = cx + nx*5.0
cy2 = cy + ny*5.0
# Draw center of the screen
$gtk.args.outputs.sprites << {x: 640-3, y: 360-3, w: 6, h: 6, path: 'sprites/circle/white.png' }
$gtk.args.outputs.lines << { x: point_ax + 640, y: point_ay + 360, x2: point_bx + 640, y2: point_by + 360, r: 255, g: 255, b: 255 }
#$gtk.args.outputs.lines << { x: cx + 640, y: cy + 360, x2: cx2 + 640, y2: cy2 + 360, r: 255, g: 0, b: 255 }
end
if (point_ay - point_cy)*(point_cx + point_ax) > (point_ax - point_cx)*(point_cy + point_ay) &&
(point_cy - point_by)*(point_bx + point_cx) > (point_cx - point_bx)*(point_by + point_cy)
# The triangle three points contains the origin so we're overlapping
# Begin EPA algorithm
# For now, just return true that there was a collision
$gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
break
else
# check axis point_a, point_b, point_c, n
if (point_cx*dx + point_cy*dx) <= [(point_ax*dx + point_ay*dy), (point_bx*dx + point_by*dy)].max
break
else
# ClosestDist point_a, point_c < ClosestDistance point_c, point_b
# Closest_t for point_a, point_c
sx = point_cx - point_ax
sy = point_cy - point_ay
t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)
t *= 0.5
# lerp point_a, point_c
ac_x = (point_ax*(0.5 - t)) + (point_cx*(0.5 + t))
ac_y = (point_ay*(0.5 - t)) + (point_cy*(0.5 + t))
ac_dist = ac_x*ac_x + ac_y*ac_y
# closest_t for point_c, point_b
sx = point_bx - point_cx
sy = point_bx - point_cx
t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)
t *= 0.5
# lerp point_c, point_b
cb_x = (point_cx*(0.5 - t)) + (point_bx*(0.5 + t))
cb_y = (point_cy*(0.5 - t)) + (point_by*(0.5 + t))
cb_dist = cb_x*cb_x + cb_y*cb_y
if ac_dist < cb_dist
point_bx = point_cx
point_by = point_cy
else
point_ax = point_cx
point_ay = point_cy
end
end
end
i += 1
end
if @debug_draw
$gtk.args.outputs.lines << { x: point_ax+640, y: point_ay+360, x2: point_bx+640, y2: point_by+360, r: 255, g: 0, b: 0 }
end
# For now just return false if we got here
false
end
def calc_collision collision
end
end
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment