1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
|
// This file is part of the 64k demo project.
// It implements a lightweight SDF-based physics engine.
#include "3d/physics.h"
#include "3d/bvh.h"
#include "3d/sdf_cpu.h"
#include <algorithm>
// get_world_aabb() is declared in bvh.h
float PhysicsSystem::sample_sdf(const Object3D& obj, vec3 world_p) {
mat4 inv_model = obj.get_model_matrix().inverse();
vec4 local_p4 = inv_model * vec4(world_p.x, world_p.y, world_p.z, 1.0f);
vec3 q = local_p4.xyz();
float d = 1000.0f;
if (obj.type == ObjectType::SPHERE) {
d = q.len() - 1.0f;
} else if (obj.type == ObjectType::BOX || obj.type == ObjectType::CUBE) {
d = sdf::sdBox(q, vec3(1.0f, 1.0f, 1.0f));
} else if (obj.type == ObjectType::TORUS) {
d = sdf::sdTorus(q, vec2(1.0f, 0.4f));
} else if (obj.type == ObjectType::PLANE) {
d = sdf::sdPlane(q, vec3(0.0f, 1.0f, 0.0f), 0.0f);
}
// Extract scale from model matrix (assuming orthogonal with uniform or
// non-uniform scale)
mat4 model = obj.get_model_matrix();
float sx = vec3(model.m[0], model.m[1], model.m[2]).len();
float sy = vec3(model.m[4], model.m[5], model.m[6]).len();
float sz = vec3(model.m[8], model.m[9], model.m[10]).len();
float s = std::min(sx, std::min(sy, sz));
if (obj.type == ObjectType::PLANE) {
s = sy; // For plane with local normal (0,1,0), scale is sy
}
return d * s;
}
void PhysicsSystem::resolve_collision(Object3D& a, Object3D& b) {
if (a.is_static && b.is_static)
return;
// Probe points for 'a' (center and corners)
BoundingVolume local = a.get_local_bounds();
mat4 model_a = a.get_model_matrix();
vec3 probes[9] = {
{0, 0, 0}, // Center
{local.min.x, local.min.y, local.min.z},
{local.max.x, local.min.y, local.min.z},
{local.min.x, local.max.y, local.min.z},
{local.max.x, local.max.y, local.min.z},
{local.min.x, local.min.y, local.max.z},
{local.max.x, local.min.y, local.max.z},
{local.min.x, local.max.y, local.max.z},
{local.max.x, local.max.y, local.max.z},
};
for (int i = 0; i < 9; ++i) {
vec3 world_probe =
(model_a * vec4(probes[i].x, probes[i].y, probes[i].z, 1.0f)).xyz();
float d = sample_sdf(b, world_probe);
if (d < 0.0f) {
// Collision detected!
float penetration = -d;
// Calculate normal via gradient of b's SDF
auto b_sdf = [this, &b](vec3 p) { return sample_sdf(b, p); };
vec3 normal = sdf::calc_normal(world_probe, b_sdf);
// Resolution
if (!a.is_static) {
// Positional correction
a.position += normal * penetration;
// Velocity response
float v_dot_n = vec3::dot(a.velocity, normal);
if (v_dot_n < 0) {
a.velocity -= normal * (1.0f + a.restitution) * v_dot_n;
}
}
}
}
}
void PhysicsSystem::update(Scene& scene, float dt) {
if (dt <= 0)
return;
// 1. Integration
for (auto& obj : scene.objects) {
if (obj.is_static)
continue;
obj.velocity += gravity * dt;
obj.position += obj.velocity * dt;
}
// 2. Broad Phase
BVH bvh;
BVHBuilder::build(bvh, scene.objects);
// 3. Narrow Phase & Resolution
// We do multiple iterations for better stability? Just 1 for now.
for (int iter = 0; iter < 2; ++iter) {
for (int i = 0; i < (int)scene.objects.size(); ++i) {
Object3D& a = scene.objects[i];
if (a.is_static)
continue;
AABB query_box = get_world_aabb(a);
std::vector<int> candidates;
bvh.query(query_box, candidates);
for (int cand_idx : candidates) {
if (cand_idx == i)
continue;
resolve_collision(a, scene.objects[cand_idx]);
}
}
}
}
|
