Refactored HullScene making tests easier to visualize.
Martin Felis
parent
1adf0c611f
commit
7830840d81
|
|
@ -524,7 +524,7 @@ float sch_query_face_directions(
|
|||
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
|
||||
|
||||
simd4f vert_A;
|
||||
simd4x4f_inv_ortho_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
|
||||
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
|
||||
float distance = sch_plane_distance(&plane, &vert_A);
|
||||
if (distance > result->dist) {
|
||||
result->dist = distance;
|
||||
|
|
@ -730,7 +730,9 @@ bool sch_hull_sat(
|
|||
}
|
||||
|
||||
sch_face_query query_B_A;
|
||||
sch_query_face_directions(hull_B, &trans_BtoA, hull_A, &query_B_A);
|
||||
simd4x4f trans_AtoB;
|
||||
sch_simd4x4f_transform_inv(&trans_BtoA, &trans_AtoB);
|
||||
sch_query_face_directions(hull_B, &trans_AtoB, hull_A, &query_B_A);
|
||||
if (query_B_A.dist > 0.f) {
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
308
src/vissim.cc
308
src/vissim.cc
|
|
@ -471,92 +471,62 @@ void SimulatorSceneRender(srcmdbuf* render_commands) {
|
|||
}
|
||||
}
|
||||
|
||||
sch_hull box_hull;
|
||||
srmeshdata* box_hull_mesh;
|
||||
simd4x4f box_hull_trasform {
|
||||
simd4f_create(1.f, 0.f, 0.0, 0.f),
|
||||
simd4f_create(0.f, 1.f, 0.f, 0.f),
|
||||
simd4f_create(0.f, 0.f, 1.f, 0.f),
|
||||
simd4f_create(0.f, 0.f, 0.f, 1.f)};
|
||||
struct SceneHull {
|
||||
sch_hull hull;
|
||||
srmeshdata* mesh = nullptr;
|
||||
simd4x4f transform = {
|
||||
simd4f_create(1.f, 0.f, 0.0, 0.f),
|
||||
simd4f_create(0.f, 1.f, 0.f, 0.f),
|
||||
simd4f_create(0.f, 0.f, 1.f, 0.f),
|
||||
simd4f_create(0.f, 0.f, 0.f, 1.f)};
|
||||
|
||||
void SconvHUllSceneInit() {
|
||||
sch_create_unitbox(&box_hull);
|
||||
~SceneHull() {
|
||||
if (mesh != nullptr) {
|
||||
destroy();
|
||||
}
|
||||
};
|
||||
|
||||
// create mesh for hull
|
||||
static srvrtxdata mesh_buffer_data[1024];
|
||||
static GLuint index_buffer_data[1024];
|
||||
int vertex_index = -1;
|
||||
int index_count = -1;
|
||||
int triangle_count = 0;
|
||||
void destroy() {
|
||||
srmesh_destroy(mesh);
|
||||
mesh = nullptr;
|
||||
}
|
||||
|
||||
sch_hull* hull = &box_hull;
|
||||
void createMesh() {
|
||||
assert(mesh == nullptr);
|
||||
|
||||
int face_vert0_idx = 0;
|
||||
// create mesh for hull
|
||||
static srvrtxdata mesh_buffer_data[1024];
|
||||
static GLuint index_buffer_data[1024];
|
||||
int vertex_index = -1;
|
||||
int index_count = -1;
|
||||
int triangle_count = 0;
|
||||
|
||||
for (int fi = 0; fi < hull->num_faces; fi++) {
|
||||
sch_face* face = &hull->faces[fi];
|
||||
sch_edge* edge0 = face->edge;
|
||||
sch_vert* face_vert0 = edge0->vert;
|
||||
sch_edge* cur_edge = edge0->next;
|
||||
sch_vert* cur_vert = nullptr;
|
||||
sch_edge* next_edge = nullptr;
|
||||
sch_vert* next_vert = nullptr;
|
||||
int face_vert0_idx = 0;
|
||||
|
||||
int tri_vert_0_idx = 0;
|
||||
int tri_vert_1_idx = 0;
|
||||
int tri_vert_2_idx = 0;
|
||||
for (int fi = 0; fi < hull.num_faces; fi++) {
|
||||
sch_face* face = &hull.faces[fi];
|
||||
sch_edge* edge0 = face->edge;
|
||||
sch_vert* face_vert0 = edge0->vert;
|
||||
sch_edge* cur_edge = edge0->next;
|
||||
sch_vert* cur_vert = nullptr;
|
||||
sch_edge* next_edge = nullptr;
|
||||
sch_vert* next_vert = nullptr;
|
||||
|
||||
sch_plane plane;
|
||||
sch_hull_calc_plane(hull, fi, &plane);
|
||||
int tri_vert_0_idx = 0;
|
||||
int tri_vert_1_idx = 0;
|
||||
int tri_vert_2_idx = 0;
|
||||
|
||||
// add face_vert0 (later used as tri_vert_0)
|
||||
vertex_index++;
|
||||
face_vert0_idx = vertex_index;
|
||||
tri_vert_0_idx = face_vert0_idx;
|
||||
srvrtxdata* mesh_vert = &mesh_buffer_data[tri_vert_0_idx];
|
||||
mesh_vert->x = simd4f_get_x(face_vert0->p);
|
||||
mesh_vert->y = simd4f_get_y(face_vert0->p);
|
||||
mesh_vert->z = simd4f_get_z(face_vert0->p);
|
||||
mesh_vert->w = 1.f;
|
||||
mesh_vert->nx = simd4f_get_x(plane.n);
|
||||
mesh_vert->ny = simd4f_get_y(plane.n);
|
||||
mesh_vert->nz = simd4f_get_z(plane.n);
|
||||
mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
|
||||
mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
|
||||
mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
|
||||
mesh_vert->color[3] = 255;
|
||||
sch_plane plane;
|
||||
sch_hull_calc_plane(&hull, fi, &plane);
|
||||
|
||||
// add tri_vert_1
|
||||
cur_vert = cur_edge->vert;
|
||||
tri_vert_1_idx = ++vertex_index;
|
||||
mesh_vert = &mesh_buffer_data[tri_vert_1_idx];
|
||||
mesh_vert->x = simd4f_get_x(cur_vert->p);
|
||||
mesh_vert->y = simd4f_get_y(cur_vert->p);
|
||||
mesh_vert->z = simd4f_get_z(cur_vert->p);
|
||||
mesh_vert->w = 1.f;
|
||||
mesh_vert->nx = simd4f_get_x(plane.n);
|
||||
mesh_vert->ny = simd4f_get_y(plane.n);
|
||||
mesh_vert->nz = simd4f_get_z(plane.n);
|
||||
mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
|
||||
mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
|
||||
mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
|
||||
mesh_vert->color[3] = 255;
|
||||
|
||||
do {
|
||||
// add face_vert0
|
||||
index_buffer_data[++index_count] = tri_vert_0_idx;
|
||||
|
||||
// add face_vert1
|
||||
index_buffer_data[++index_count] = tri_vert_1_idx;
|
||||
|
||||
next_edge = cur_edge->next;
|
||||
next_vert = next_edge->vert;
|
||||
tri_vert_2_idx = ++vertex_index;
|
||||
|
||||
mesh_vert = &mesh_buffer_data[tri_vert_2_idx];
|
||||
mesh_vert->x = simd4f_get_x(next_vert->p);
|
||||
mesh_vert->y = simd4f_get_y(next_vert->p);
|
||||
mesh_vert->z = simd4f_get_z(next_vert->p);
|
||||
// add face_vert0 (later used as tri_vert_0)
|
||||
vertex_index++;
|
||||
face_vert0_idx = vertex_index;
|
||||
tri_vert_0_idx = face_vert0_idx;
|
||||
srvrtxdata* mesh_vert = &mesh_buffer_data[tri_vert_0_idx];
|
||||
mesh_vert->x = simd4f_get_x(face_vert0->p);
|
||||
mesh_vert->y = simd4f_get_y(face_vert0->p);
|
||||
mesh_vert->z = simd4f_get_z(face_vert0->p);
|
||||
mesh_vert->w = 1.f;
|
||||
mesh_vert->nx = simd4f_get_x(plane.n);
|
||||
mesh_vert->ny = simd4f_get_y(plane.n);
|
||||
|
|
@ -565,70 +535,143 @@ void SconvHUllSceneInit() {
|
|||
mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
|
||||
mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
|
||||
mesh_vert->color[3] = 255;
|
||||
index_buffer_data[++index_count] = tri_vert_2_idx;
|
||||
|
||||
triangle_count++;
|
||||
cur_edge = next_edge;
|
||||
tri_vert_1_idx = tri_vert_2_idx;
|
||||
} while (next_edge->next != edge0);
|
||||
}
|
||||
// add tri_vert_1
|
||||
cur_vert = cur_edge->vert;
|
||||
tri_vert_1_idx = ++vertex_index;
|
||||
mesh_vert = &mesh_buffer_data[tri_vert_1_idx];
|
||||
mesh_vert->x = simd4f_get_x(cur_vert->p);
|
||||
mesh_vert->y = simd4f_get_y(cur_vert->p);
|
||||
mesh_vert->z = simd4f_get_z(cur_vert->p);
|
||||
mesh_vert->w = 1.f;
|
||||
mesh_vert->nx = simd4f_get_x(plane.n);
|
||||
mesh_vert->ny = simd4f_get_y(plane.n);
|
||||
mesh_vert->nz = simd4f_get_z(plane.n);
|
||||
mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
|
||||
mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
|
||||
mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
|
||||
mesh_vert->color[3] = 255;
|
||||
|
||||
box_hull_mesh = srmesh_create(
|
||||
mesh_buffer_data,
|
||||
vertex_index + 1,
|
||||
index_buffer_data,
|
||||
index_count + 1,
|
||||
GL_TRIANGLES,
|
||||
index_count + 1);
|
||||
}
|
||||
do {
|
||||
// add face_vert0
|
||||
index_buffer_data[++index_count] = tri_vert_0_idx;
|
||||
|
||||
void SconvHUllSceneCleanup() { srmesh_destroy(box_hull_mesh); }
|
||||
// add face_vert1
|
||||
index_buffer_data[++index_count] = tri_vert_1_idx;
|
||||
|
||||
void SconvHUllSceneUpdate() {}
|
||||
next_edge = cur_edge->next;
|
||||
next_vert = next_edge->vert;
|
||||
tri_vert_2_idx = ++vertex_index;
|
||||
|
||||
void SconvHullSceneRender(srcmdbuf* render_commands) {
|
||||
// Populate render commands
|
||||
srcmdbuf_clear(render_commands);
|
||||
mesh_vert = &mesh_buffer_data[tri_vert_2_idx];
|
||||
mesh_vert->x = simd4f_get_x(next_vert->p);
|
||||
mesh_vert->y = simd4f_get_y(next_vert->p);
|
||||
mesh_vert->z = simd4f_get_z(next_vert->p);
|
||||
mesh_vert->w = 1.f;
|
||||
mesh_vert->nx = simd4f_get_x(plane.n);
|
||||
mesh_vert->ny = simd4f_get_y(plane.n);
|
||||
mesh_vert->nz = simd4f_get_z(plane.n);
|
||||
mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
|
||||
mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
|
||||
mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
|
||||
mesh_vert->color[3] = 255;
|
||||
index_buffer_data[++index_count] = tri_vert_2_idx;
|
||||
|
||||
srcmd rcmd;
|
||||
srcmd_clear(&rcmd);
|
||||
|
||||
rcmd.type = SRndrCmdTypeFrame;
|
||||
srcmdbuf_add(render_commands, &rcmd);
|
||||
|
||||
rcmd.type = SRndrCmdTypeGrid;
|
||||
float origin[3] = {0.f, 0.f, 0.f};
|
||||
|
||||
for (int i = -1; i <= 1; i++) {
|
||||
for (int j = -1; j <= 1; j++) {
|
||||
simd4x4f_translation(
|
||||
&rcmd.mat,
|
||||
origin[0] + i * 10.0f,
|
||||
origin[1],
|
||||
origin[2] + j * 10.f);
|
||||
srcmdbuf_add(render_commands, &rcmd);
|
||||
triangle_count++;
|
||||
cur_edge = next_edge;
|
||||
tri_vert_1_idx = tri_vert_2_idx;
|
||||
} while (next_edge->next != edge0);
|
||||
}
|
||||
|
||||
mesh = srmesh_create(
|
||||
mesh_buffer_data,
|
||||
vertex_index + 1,
|
||||
index_buffer_data,
|
||||
index_count + 1,
|
||||
GL_TRIANGLES,
|
||||
index_count + 1);
|
||||
}
|
||||
};
|
||||
|
||||
struct SconvHullScene {
|
||||
SceneHull mHullA;
|
||||
SceneHull mHullB;
|
||||
SceneHull* mCurrentHull;
|
||||
|
||||
void init() {
|
||||
sch_create_unitbox(&mHullA.hull);
|
||||
mHullA.createMesh();
|
||||
|
||||
sch_create_unitbox(&mHullB.hull);
|
||||
mHullB.createMesh();
|
||||
|
||||
simd4x4f rot;
|
||||
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
|
||||
|
||||
simd4x4f translation;
|
||||
simd4x4f_translation(&translation, -0.5f - 0.51f * sqrtf(2.), 0.0f, 0.f);
|
||||
|
||||
simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
|
||||
|
||||
mCurrentHull = &mHullA;
|
||||
};
|
||||
|
||||
void destroy() {
|
||||
mHullA.destroy();
|
||||
mHullB.destroy();
|
||||
}
|
||||
|
||||
void draw(srcmdbuf* render_commands) {
|
||||
// Populate render commands
|
||||
srcmdbuf_clear(render_commands);
|
||||
|
||||
ImGuizmo::Manipulate(
|
||||
(float*)&(gCameraState->mtxView),
|
||||
(float*)&(gProjMatrix),
|
||||
ImGuizmo::TRANSLATE,
|
||||
ImGuizmo::WORLD,
|
||||
(float*)(&box_hull_trasform),
|
||||
NULL
|
||||
);
|
||||
srcmd rcmd;
|
||||
srcmd_clear(&rcmd);
|
||||
|
||||
// Render hull
|
||||
srcmd cmd;
|
||||
cmd.type = SrndrCmdTypeMesh;
|
||||
cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
|
||||
cmd.mat = box_hull_trasform;
|
||||
cmd.meshdata = box_hull_mesh;
|
||||
rcmd.type = SRndrCmdTypeFrame;
|
||||
srcmdbuf_add(render_commands, &rcmd);
|
||||
|
||||
srcmdbuf_add(render_commands, &cmd);
|
||||
}
|
||||
rcmd.type = SRndrCmdTypeGrid;
|
||||
float origin[3] = {0.f, 0.f, 0.f};
|
||||
|
||||
for (int i = -1; i <= 1; i++) {
|
||||
for (int j = -1; j <= 1; j++) {
|
||||
simd4x4f_translation(
|
||||
&rcmd.mat,
|
||||
origin[0] + i * 10.0f,
|
||||
origin[1],
|
||||
origin[2] + j * 10.f);
|
||||
srcmdbuf_add(render_commands, &rcmd);
|
||||
}
|
||||
}
|
||||
|
||||
ImGuizmo::Manipulate(
|
||||
(float*)&(gCameraState->mtxView),
|
||||
(float*)&(gProjMatrix),
|
||||
ImGuizmo::TRANSLATE,
|
||||
ImGuizmo::WORLD,
|
||||
(float*)(&mCurrentHull->transform),
|
||||
NULL);
|
||||
|
||||
// Render hulls
|
||||
srcmd cmd;
|
||||
cmd.type = SrndrCmdTypeMesh;
|
||||
cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
|
||||
cmd.mat = mHullA.transform;
|
||||
cmd.meshdata = mHullA.mesh;
|
||||
|
||||
srcmdbuf_add(render_commands, &cmd);
|
||||
|
||||
cmd.type = SrndrCmdTypeMesh;
|
||||
cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
|
||||
cmd.mat = mHullB.transform;
|
||||
cmd.meshdata = mHullB.mesh;
|
||||
|
||||
srcmdbuf_add(render_commands, &cmd);
|
||||
}
|
||||
};
|
||||
|
||||
SconvHullScene gHullScene;
|
||||
|
||||
void ImSRenderWidget(srndr* renderer, srview* view) {
|
||||
ZoneScoped;
|
||||
|
|
@ -777,7 +820,7 @@ int main() {
|
|||
// Scene init
|
||||
//
|
||||
simulator_init();
|
||||
SconvHUllSceneInit();
|
||||
gHullScene.init();
|
||||
|
||||
simd4x4f_translation(&gCameraState->mtxView, 0.1f, 0.1f, -0.5f);
|
||||
simd4f eye = simd4f_create(5.f, 4.f, 5.f, 1.f);
|
||||
|
|
@ -929,14 +972,13 @@ int main() {
|
|||
ImGui::End();
|
||||
}
|
||||
|
||||
SconvHullSceneRender(gRndrCmds);
|
||||
gHullScene.draw(gRndrCmds);
|
||||
// SimulatorSceneRender(gRndrCmds);
|
||||
srndr_render(gRndr, gView, gRndrCmds);
|
||||
ImSRenderWidget(gRndr, gView);
|
||||
}
|
||||
|
||||
simulator_update(gTimer->mDeltaTime);
|
||||
SconvHUllSceneUpdate();
|
||||
|
||||
{
|
||||
ZoneNamedN(render_submit, "RenderSubmit", true);
|
||||
|
|
@ -986,7 +1028,7 @@ int main() {
|
|||
|
||||
gLog("Exiting application");
|
||||
|
||||
SconvHUllSceneCleanup();
|
||||
gHullScene.destroy();
|
||||
|
||||
srview_destroy(gView);
|
||||
srndr_destroy(gRndr);
|
||||
|
|
|
|||
|
|
@ -350,13 +350,12 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
|
|||
sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
|
||||
|
||||
GIVEN ("Two unit boxes separated by 1.1 along x axis") {
|
||||
simd4f r = simd4f_create(0.5f + 0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
|
||||
simd4x4f translation;
|
||||
simd4x4f_translation(
|
||||
&translation,
|
||||
simd4f_get_x(r),
|
||||
simd4f_get_y(r),
|
||||
simd4f_get_z(r));
|
||||
0.5f + 0.4f * sqrtf(2.),
|
||||
0.f,
|
||||
0.f);
|
||||
|
||||
THEN("Boxes separated") {
|
||||
sch_hull_transform(&hull_B, translation);
|
||||
|
|
@ -374,7 +373,7 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
|
|||
}
|
||||
|
||||
THEN("Boxes separated when supplying trans_A") {
|
||||
simd4f r = simd4f_create(-0.5f -0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
|
||||
simd4f r = simd4f_create(-0.5f -0.51f * sqrtf(2.), 0.f, 0.f, 1.f);
|
||||
simd4x4f_translation(
|
||||
&translation,
|
||||
simd4f_get_x(r),
|
||||
|
|
@ -386,6 +385,41 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
|
|||
bool separated = sch_hull_sat(&hull_A, &translation, &hull_B, &identity, &manifold);
|
||||
REQUIRE(separated);
|
||||
}
|
||||
|
||||
THEN("Boxes separated when supplying rot_trans to B") {
|
||||
simd4x4f identity;
|
||||
simd4x4f_identity(&identity);
|
||||
|
||||
simd4x4f rot;
|
||||
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
|
||||
|
||||
simd4x4f translation;
|
||||
simd4x4f_translation(&translation, 0.5f + 0.51f * sqrtf(2.), 0.0f, 0.f);
|
||||
|
||||
simd4x4f rot_trans;
|
||||
simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
|
||||
|
||||
bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &rot_trans, &manifold);
|
||||
REQUIRE(separated);
|
||||
}
|
||||
|
||||
|
||||
THEN("Boxes separated when supplying rot_trans to A") {
|
||||
simd4x4f identity;
|
||||
simd4x4f_identity(&identity);
|
||||
|
||||
simd4x4f rot;
|
||||
simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
|
||||
|
||||
simd4x4f translation;
|
||||
simd4x4f_translation(&translation, -0.5f - 0.51f * sqrtf(2.), 0.0f, 0.f);
|
||||
|
||||
simd4x4f rot_trans;
|
||||
simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
|
||||
|
||||
bool separated = sch_hull_sat(&hull_A, &rot_trans, &hull_B, &identity, &manifold);
|
||||
REQUIRE(separated);
|
||||
}
|
||||
}
|
||||
|
||||
GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
|
||||
|
|
|
|||
Loading…
Reference in New Issue