Skip to content

Commit

Permalink
Merge pull request #59995 from Scony/fix-threaded-navmesh-baking
Browse files Browse the repository at this point in the history 
Fix navmesh baking
  • Loading branch information
@akien-mga
akien-mga authored Apr 8, 2022
2 parents 7fff643 + fcd26b8 commit eb6ebdc
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 50 deletions.
104 changes: 54 additions & 50 deletions modules/navigation/navigation_mesh_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,28 @@ void NavigationMeshGenerator::_add_mesh(const Ref<Mesh> &p_mesh, const Transform
}
}

void NavigationMeshGenerator::_add_mesh_array(const Array &p_array, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices) {
Vector<Vector3> mesh_vertices = p_array[Mesh::ARRAY_VERTEX];
const Vector3 *vr = mesh_vertices.ptr();

Vector<int> mesh_indices = p_array[Mesh::ARRAY_INDEX];
const int *ir = mesh_indices.ptr();

const int face_count = mesh_indices.size() / 3;
const int current_vertex_count = p_vertices.size() / 3;

for (int j = 0; j < mesh_vertices.size(); j++) {
_add_vertex(p_xform.xform(vr[j]), p_vertices);
}

for (int j = 0; j < face_count; j++) {
// CCW
p_indices.push_back(current_vertex_count + (ir[j * 3 + 0]));
p_indices.push_back(current_vertex_count + (ir[j * 3 + 2]));
p_indices.push_back(current_vertex_count + (ir[j * 3 + 1]));
}
}

void NavigationMeshGenerator::_add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices) {
int face_count = p_faces.size() / 3;
int current_vertex_count = p_vertices.size() / 3;
Expand Down Expand Up @@ -187,43 +209,38 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans

Transform3D transform = p_navmesh_transform * static_body->get_global_transform() * col_shape->get_transform();

Ref<Mesh> mesh;
Ref<Shape3D> s = col_shape->get_shape();

BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
if (box) {
Ref<BoxMesh> box_mesh;
box_mesh.instantiate();
box_mesh->set_size(box->get_size());
mesh = box_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, box->get_size());
_add_mesh_array(arr, transform, p_vertices, p_indices);
}

CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
if (capsule) {
Ref<CapsuleMesh> capsule_mesh;
capsule_mesh.instantiate();
capsule_mesh->set_radius(capsule->get_radius());
capsule_mesh->set_height(capsule->get_height());
mesh = capsule_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height());
_add_mesh_array(arr, transform, p_vertices, p_indices);
}

CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
if (cylinder) {
Ref<CylinderMesh> cylinder_mesh;
cylinder_mesh.instantiate();
cylinder_mesh->set_height(cylinder->get_height());
cylinder_mesh->set_bottom_radius(cylinder->get_radius());
cylinder_mesh->set_top_radius(cylinder->get_radius());
mesh = cylinder_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height());
_add_mesh_array(arr, transform, p_vertices, p_indices);
}

SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
if (sphere) {
Ref<SphereMesh> sphere_mesh;
sphere_mesh.instantiate();
sphere_mesh->set_radius(sphere->get_radius());
sphere_mesh->set_height(sphere->get_radius() * 2.0);
mesh = sphere_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0);
_add_mesh_array(arr, transform, p_vertices, p_indices);
}

ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
Expand Down Expand Up @@ -254,10 +271,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans
_add_faces(faces, transform, p_vertices, p_indices);
}
}

if (mesh.is_valid()) {
_add_mesh(mesh, transform, p_vertices, p_indices);
}
}
}
}
Expand All @@ -284,44 +297,39 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans
RID shape = shapes[i + 1];
PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape);
Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape);
Ref<Mesh> mesh;

switch (type) {
case PhysicsServer3D::SHAPE_SPHERE: {
real_t radius = data;
Ref<SphereMesh> sphere_mesh;
sphere_mesh.instantiate();
sphere_mesh->set_radius(radius);
sphere_mesh->set_height(radius * 2.0);
mesh = sphere_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, radius, radius * 2.0);
_add_mesh_array(arr, shapes[i], p_vertices, p_indices);
} break;
case PhysicsServer3D::SHAPE_BOX: {
Vector3 extents = data;
Ref<BoxMesh> box_mesh;
box_mesh.instantiate();
box_mesh->set_size(2.0 * extents);
mesh = box_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, extents * 2.0);
_add_mesh_array(arr, shapes[i], p_vertices, p_indices);
} break;
case PhysicsServer3D::SHAPE_CAPSULE: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Ref<CapsuleMesh> capsule_mesh;
capsule_mesh.instantiate();
capsule_mesh->set_radius(radius);
capsule_mesh->set_height(height);
mesh = capsule_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, radius, height);
_add_mesh_array(arr, shapes[i], p_vertices, p_indices);
} break;
case PhysicsServer3D::SHAPE_CYLINDER: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Ref<CylinderMesh> cylinder_mesh;
cylinder_mesh.instantiate();
cylinder_mesh->set_height(height);
cylinder_mesh->set_bottom_radius(radius);
cylinder_mesh->set_top_radius(radius);
mesh = cylinder_mesh;
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, radius, radius, height);
_add_mesh_array(arr, shapes[i], p_vertices, p_indices);
} break;
case PhysicsServer3D::SHAPE_CONVEX_POLYGON: {
PackedVector3Array vertices = data;
Expand Down Expand Up @@ -354,10 +362,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans
WARN_PRINT("Unsupported collision shape type.");
} break;
}

if (mesh.is_valid()) {
_add_mesh(mesh, shapes[i], p_vertices, p_indices);
}
}
}
}
Expand Down
1 change: 1 addition & 0 deletions modules/navigation/navigation_mesh_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ class NavigationMeshGenerator : public Object {

static void _add_vertex(const Vector3 &p_vec3, Vector<float> &p_vertices);
static void _add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices);
static void _add_mesh_array(const Array &p_array, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices);
static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices);
static void _parse_geometry(const Transform3D &p_navmesh_transform, Node *p_node, Vector<float> &p_vertices, Vector<int> &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children);

Expand Down
16 changes: 16 additions & 0 deletions scene/resources/primitive_meshes.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,6 +270,10 @@ PrimitiveMesh::~PrimitiveMesh() {
*/

void CapsuleMesh::_create_mesh_array(Array &p_arr) const {
create_mesh_array(p_arr, radius, height, radial_segments, rings);
}

void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const float height, const int radial_segments, const int rings) {
int i, j, prevrow, thisrow, point;
float x, y, z, u, v, w;
float onethird = 1.0 / 3.0;
Expand Down Expand Up @@ -481,6 +485,10 @@ CapsuleMesh::CapsuleMesh() {}
*/

void BoxMesh::_create_mesh_array(Array &p_arr) const {
BoxMesh::create_mesh_array(p_arr, size, subdivide_w, subdivide_h, subdivide_d);
}

void BoxMesh::create_mesh_array(Array &p_arr, Vector3 size, int subdivide_w, int subdivide_h, int subdivide_d) {
int i, j, prevrow, thisrow, point;
float x, y, z;
float onethird = 1.0 / 3.0;
Expand Down Expand Up @@ -732,6 +740,10 @@ BoxMesh::BoxMesh() {}
*/

void CylinderMesh::_create_mesh_array(Array &p_arr) const {
create_mesh_array(p_arr, top_radius, bottom_radius, height, radial_segments, rings);
}

void CylinderMesh::create_mesh_array(Array &p_arr, float top_radius, float bottom_radius, float height, int radial_segments, int rings) {
int i, j, prevrow, thisrow, point;
float x, y, z, u, v, radius;

Expand Down Expand Up @@ -1431,6 +1443,10 @@ Vector3 QuadMesh::get_center_offset() const {
*/

void SphereMesh::_create_mesh_array(Array &p_arr) const {
create_mesh_array(p_arr, radius, height, radial_segments, rings, is_hemisphere);
}

void SphereMesh::create_mesh_array(Array &p_arr, float radius, float height, int radial_segments, int rings, bool is_hemisphere) {
int i, j, prevrow, thisrow, point;
float x, y, z;

Expand Down
8 changes: 8 additions & 0 deletions scene/resources/primitive_meshes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,8 @@ class CapsuleMesh : public PrimitiveMesh {
virtual void _create_mesh_array(Array &p_arr) const override;

public:
static void create_mesh_array(Array &p_arr, float radius, float height, int radial_segments = 64, int rings = 8);

void set_radius(const float p_radius);
float get_radius() const;

Expand Down Expand Up @@ -149,6 +151,8 @@ class BoxMesh : public PrimitiveMesh {
virtual void _create_mesh_array(Array &p_arr) const override;

public:
static void create_mesh_array(Array &p_arr, Vector3 size, int subdivide_w = 0, int subdivide_h = 0, int subdivide_d = 0);

void set_size(const Vector3 &p_size);
Vector3 get_size() const;

Expand Down Expand Up @@ -183,6 +187,8 @@ class CylinderMesh : public PrimitiveMesh {
virtual void _create_mesh_array(Array &p_arr) const override;

public:
static void create_mesh_array(Array &p_arr, float top_radius, float bottom_radius, float height, int radial_segments = 64, int rings = 4);

void set_top_radius(const float p_radius);
float get_top_radius() const;

Expand Down Expand Up @@ -314,6 +320,8 @@ class SphereMesh : public PrimitiveMesh {
virtual void _create_mesh_array(Array &p_arr) const override;

public:
static void create_mesh_array(Array &p_arr, float radius, float height, int radial_segments = 64, int rings = 32, bool is_hemisphere = false);

void set_radius(const float p_radius);
float get_radius() const;

Expand Down

0 comments on commit eb6ebdc

Please sign in to comment.