Edge contacts and fix bug with face contacts
This commit is contained in:
parent
4af30979d5
commit
82470ca3c7
@ -495,9 +495,11 @@ draw :: proc() {
|
||||
box1_mat := linalg.Matrix4f32(1)
|
||||
box1_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 1, 0}) * box1_mat
|
||||
box2_mat := linalg.Matrix4f32(1)
|
||||
box2_mat = linalg.matrix4_translate(rl.Vector3{0.5, 0.2, 0}) * box2_mat
|
||||
// box2_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 0, 1}) * box2_mat
|
||||
// box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, -1, 0}) * box2_mat
|
||||
box2_mat = linalg.matrix4_translate(rl.Vector3{0.5, 0.5, 0}) * box2_mat
|
||||
box2_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 0, 1}) * box2_mat
|
||||
box2_mat = linalg.matrix4_translate(rl.Vector3{1.35, -0.5, 0}) * box2_mat
|
||||
box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, 1, 0}) * box2_mat
|
||||
|
||||
|
||||
box1, box2 := collision.Box {
|
||||
|
||||
@ -82,8 +82,11 @@ mesh_from_vertex_index_list :: proc(
|
||||
assert(abs(dist - point_dist) < 0.00001)
|
||||
}
|
||||
|
||||
if i == 0 {
|
||||
if verts[index].edge == -1 {
|
||||
verts[index].edge = Edge_Index(e)
|
||||
}
|
||||
|
||||
if i == 0 {
|
||||
faces[f].edge = Edge_Index(e)
|
||||
}
|
||||
|
||||
@ -165,16 +168,23 @@ iterator_reset_edges :: proc(it: ^Edge_Iterator) {
|
||||
it.past_first = false
|
||||
}
|
||||
|
||||
iterate_next_edge :: proc(it: ^Edge_Iterator) -> (edge: Half_Edge, ok: bool) {
|
||||
iterate_next_edge :: proc(
|
||||
it: ^Edge_Iterator,
|
||||
) -> (
|
||||
edge: Half_Edge,
|
||||
edge_idx: Edge_Index,
|
||||
ok: bool,
|
||||
) {
|
||||
if it.current_edge == it.first_edge {
|
||||
if !it.past_first {
|
||||
it.past_first = true
|
||||
} else {
|
||||
return {}, false
|
||||
return {}, -1, false
|
||||
}
|
||||
}
|
||||
|
||||
edge = it.mesh.edges[it.current_edge]
|
||||
edge_idx = it.current_edge
|
||||
ok = true
|
||||
it.current_edge = edge.next
|
||||
|
||||
@ -195,9 +205,8 @@ transform_mesh :: proc(mesh: ^Half_Edge_Mesh, mat: lg.Matrix4f32) {
|
||||
mesh.center = new_center
|
||||
|
||||
for i in 0 ..< len(mesh.faces) {
|
||||
face := &mesh.faces[i]
|
||||
n := face.normal
|
||||
face.normal = lg.normalize0((mat * Vec4{n.x, n.y, n.z, 0}).xyz)
|
||||
n := &mesh.faces[i].normal
|
||||
mesh.faces[i].normal = lg.normalize0((mat * Vec4{n.x, n.y, n.z, 0}).xyz)
|
||||
}
|
||||
}
|
||||
|
||||
@ -213,3 +222,53 @@ get_face_centroid :: proc(mesh: Half_Edge_Mesh, face_idx: Face_Index) -> Vec3 {
|
||||
|
||||
return center
|
||||
}
|
||||
|
||||
Vertex_Edge_Iterator :: struct {
|
||||
mesh: Half_Edge_Mesh,
|
||||
vert: Vertex_Index,
|
||||
first_edge_idx: Edge_Index,
|
||||
edge_idx: Edge_Index,
|
||||
iteration: i32,
|
||||
}
|
||||
|
||||
// Iterates over all edges that have vert_idx as the origin
|
||||
iterator_vertex_edges :: proc(
|
||||
mesh: Half_Edge_Mesh,
|
||||
vert_idx: Vertex_Index,
|
||||
) -> (
|
||||
it: Vertex_Edge_Iterator,
|
||||
) {
|
||||
it.mesh = mesh
|
||||
it.vert = vert_idx
|
||||
it.first_edge_idx = mesh.vertices[vert_idx].edge
|
||||
it.edge_idx = it.first_edge_idx
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
iterate_next_vertex_edge :: proc(
|
||||
it: ^Vertex_Edge_Iterator,
|
||||
) -> (
|
||||
edge: Half_Edge,
|
||||
edge_idx: Edge_Index,
|
||||
ok: bool,
|
||||
) {
|
||||
if it.edge_idx != -1 && (it.edge_idx != it.first_edge_idx || it.iteration == 0) {
|
||||
edge = it.mesh.edges[it.edge_idx]
|
||||
edge_idx = it.edge_idx
|
||||
ok = true
|
||||
|
||||
twin := edge.twin
|
||||
if twin >= 0 {
|
||||
it.edge_idx = it.mesh.edges[twin].next
|
||||
} else {
|
||||
it.edge_idx = -1
|
||||
}
|
||||
|
||||
it.iteration += 1
|
||||
} else {
|
||||
edge_idx = -1
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
@ -59,9 +59,11 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
|
||||
if edge_separation > 0 {
|
||||
return
|
||||
}
|
||||
face_query_a.separation += 0.1
|
||||
edge_separation -= 0.1
|
||||
|
||||
is_face_a_contact := face_query_a.separation > edge_separation
|
||||
is_face_b_contact := face_query_b.separation > edge_separation
|
||||
is_face_a_contact := face_query_a.separation >= edge_separation
|
||||
is_face_b_contact := face_query_b.separation >= edge_separation
|
||||
|
||||
log.infof(
|
||||
"face_a_sep: %v, face_b_sep: %v, edge_sep: %v",
|
||||
@ -93,7 +95,7 @@ query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) {
|
||||
pos := a.vertices[index].pos
|
||||
normal := face.normal
|
||||
|
||||
support_point := find_support_point(b, -normal)
|
||||
support_point, _, _ := find_support_point(b, -normal)
|
||||
|
||||
plane := plane_from_point_normal(pos, normal)
|
||||
|
||||
@ -121,18 +123,26 @@ find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> Vec3 {
|
||||
return p
|
||||
}
|
||||
|
||||
find_support_point :: proc(convex: Convex, normal: Vec3) -> halfedge.Vertex {
|
||||
p: halfedge.Vertex
|
||||
find_support_point :: proc(
|
||||
convex: Convex,
|
||||
normal: Vec3,
|
||||
) -> (
|
||||
vert: halfedge.Vertex,
|
||||
idx: halfedge.Vertex_Index,
|
||||
ok: bool,
|
||||
) {
|
||||
max_proj := min(f32)
|
||||
for vert in convex.vertices {
|
||||
proj := lg.dot(vert.pos, normal)
|
||||
for v, i in convex.vertices {
|
||||
proj := lg.dot(v.pos, normal)
|
||||
if proj > max_proj {
|
||||
max_proj = proj
|
||||
p = vert
|
||||
vert = v
|
||||
idx = halfedge.Vertex_Index(i)
|
||||
ok = true
|
||||
}
|
||||
}
|
||||
|
||||
return p
|
||||
return
|
||||
}
|
||||
|
||||
query_separation_edges :: proc(
|
||||
@ -169,8 +179,30 @@ query_separation_edges :: proc(
|
||||
axis = -axis
|
||||
}
|
||||
plane_a := plane_from_point_normal(edge_a_origin, axis)
|
||||
vert_a := find_support_point(a, plane_a.normal)
|
||||
vert_b := find_support_point(b, -plane_a.normal)
|
||||
vert_a, _, _ := find_support_point(a, plane_a.normal)
|
||||
vert_b, vert_b_idx, _ := find_support_point(b, -plane_a.normal)
|
||||
|
||||
// We found the support vert on mesh b, but now we need to find the
|
||||
// best edge that includes that point
|
||||
vert_b_edge: halfedge.Half_Edge
|
||||
vert_b_edge_idx: halfedge.Edge_Index = -1
|
||||
{
|
||||
min_b2_distance := max(f32)
|
||||
it := halfedge.iterator_vertex_edges(b, vert_b_idx)
|
||||
for edge, edge_idx in halfedge.iterate_next_vertex_edge(&it) {
|
||||
_, vert_b2 := halfedge.get_edge_points(b, edge)
|
||||
|
||||
distance_b2 := signed_distance_plane(vert_b2, plane_a)
|
||||
if distance_b2 < min_b2_distance {
|
||||
min_b2_distance = distance_b2
|
||||
vert_b_edge = edge
|
||||
vert_b_edge_idx = edge_idx
|
||||
}
|
||||
}
|
||||
|
||||
assert(vert_b_edge_idx >= 0, "couldn't find the edge on convex B")
|
||||
}
|
||||
|
||||
distance_a := signed_distance_plane(vert_a.pos, plane_a)
|
||||
if distance_a > 0 {
|
||||
continue
|
||||
@ -182,7 +214,7 @@ query_separation_edges :: proc(
|
||||
// a1, a2 := halfedge.get_edge_points(a, edge_a)
|
||||
// edge_a_center := (a1 + a2) * 0.5
|
||||
a1, a2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(a), edge_a)
|
||||
b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), edge_b)
|
||||
b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), vert_b_edge)
|
||||
|
||||
rl.DrawLine3D(edge_a_origin, edge_a_origin + plane_a.normal, rl.BLUE)
|
||||
rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
|
||||
@ -203,7 +235,7 @@ query_separation_edges :: proc(
|
||||
if distance_b > separation {
|
||||
separation = distance_b
|
||||
a_edge = halfedge.Edge_Index(edge_a_idx)
|
||||
b_edge = vert_b.edge
|
||||
b_edge = vert_b_edge_idx
|
||||
separating_plane = plane_a
|
||||
separating_plane_p = edge_a_origin
|
||||
success_step = step
|
||||
@ -231,7 +263,7 @@ create_face_contact_manifold :: proc(
|
||||
ref_convex := is_ref_a ? a : b
|
||||
inc_convex := is_ref_a ? b : a
|
||||
|
||||
ref_face := a.faces[ref_face_query.face]
|
||||
ref_face := ref_convex.faces[ref_face_query.face]
|
||||
|
||||
// incident face
|
||||
inc_face: halfedge.Face
|
||||
@ -403,9 +435,8 @@ create_edge_contact_manifold :: proc(
|
||||
|
||||
_, ps := closest_point_between_segments(a1, a2, b1, b2)
|
||||
|
||||
manifold.points = make([]Vec3, 2, context.temp_allocator)
|
||||
manifold.points[0] = ps[0]
|
||||
manifold.points[1] = ps[1]
|
||||
manifold.points = make([]Vec3, 1, context.temp_allocator)
|
||||
manifold.points[0] = (ps[0] + ps[1]) * 0.5
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user