summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/godot_area_3d.cpp6
-rw-r--r--servers/physics_3d/godot_body_3d.cpp32
-rw-r--r--servers/physics_3d/godot_body_pair_3d.cpp2
-rw-r--r--servers/physics_3d/godot_collision_solver_3d.cpp4
-rw-r--r--servers/physics_3d/godot_collision_solver_3d_sat.cpp63
-rw-r--r--servers/physics_3d/godot_shape_3d.cpp210
-rw-r--r--servers/physics_3d/godot_shape_3d.h2
-rw-r--r--servers/physics_3d/godot_soft_body_3d.cpp9
8 files changed, 215 insertions, 113 deletions
diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp
index 9765d0bf58..d4d3b3e6aa 100644
--- a/servers/physics_3d/godot_area_3d.cpp
+++ b/servers/physics_3d/godot_area_3d.cpp
@@ -333,12 +333,12 @@ void GodotArea3D::call_queries() {
void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
if (is_gravity_point()) {
- const real_t gravity_distance_scale = get_gravity_distance_scale();
+ const real_t gr_distance_scale = get_gravity_distance_scale();
Vector3 v = get_transform().xform(get_gravity_vector()) - p_position;
- if (gravity_distance_scale > 0) {
+ if (gr_distance_scale > 0) {
const real_t v_length = v.length();
if (v_length > 0) {
- const real_t v_scaled = v_length * gravity_distance_scale;
+ const real_t v_scaled = v_length * gr_distance_scale;
r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled)));
} else {
r_gravity = Vector3();
diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index 19f065c319..53f4ab86f9 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -78,10 +78,10 @@ void GodotBody3D::update_mass_properties() {
real_t area = get_shape_area(i);
- real_t mass = area * this->mass / total_area;
+ real_t mass_new = area * mass / total_area;
// NOTE: we assume that the shape origin is also its center of mass.
- center_of_mass_local += mass * get_shape_transform(i).origin;
+ center_of_mass_local += mass_new * get_shape_transform(i).origin;
}
center_of_mass_local /= mass;
@@ -108,9 +108,9 @@ void GodotBody3D::update_mass_properties() {
const GodotShape3D *shape = get_shape(i);
- real_t mass = area * this->mass / total_area;
+ real_t mass_new = area * mass / total_area;
- Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass));
+ Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new));
Transform3D shape_transform = get_shape_transform(i);
Basis shape_basis = shape_transform.basis.orthonormalized();
@@ -118,7 +118,7 @@ void GodotBody3D::update_mass_properties() {
shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
- inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
+ inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new;
}
// Set the inertia to a valid value when there are no valid shapes.
@@ -637,14 +637,14 @@ void GodotBody3D::integrate_forces(real_t p_step) {
damp = 0;
}
- real_t angular_damp = 1.0 - p_step * total_angular_damp;
+ real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
- if (angular_damp < 0) { // reached zero in the given time
- angular_damp = 0;
+ if (angular_damp_new < 0) { // reached zero in the given time
+ angular_damp_new = 0;
}
linear_velocity *= damp;
- angular_velocity *= angular_damp;
+ angular_velocity *= angular_damp_new;
linear_velocity += _inv_mass * force * p_step;
angular_velocity += _inv_inertia_tensor.xform(torque) * p_step;
@@ -707,27 +707,27 @@ void GodotBody3D::integrate_velocities(real_t p_step) {
Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;
real_t ang_vel = total_angular_velocity.length();
- Transform3D transform = get_transform();
+ Transform3D transform_new = get_transform();
if (!Math::is_zero_approx(ang_vel)) {
Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
Basis rot(ang_vel_axis, ang_vel * p_step);
Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
- transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
- transform.basis = rot * transform.basis;
- transform.orthonormalize();
+ transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local);
+ transform_new.basis = rot * transform_new.basis;
+ transform_new.orthonormalize();
}
Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
/*for(int i=0;i<3;i++) {
if (axis_lock&(1<<i)) {
- transform.origin[i]=0.0;
+ transform_new.origin[i]=0.0;
}
}*/
- transform.origin += total_linear_velocity * p_step;
+ transform_new.origin += total_linear_velocity * p_step;
- _set_transform(transform);
+ _set_transform(transform_new);
_set_inv_transform(get_transform().inverse());
_update_transform_dependent();
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index eebbe0196d..7e6cc6f834 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -170,7 +170,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
Vector3 mnormal = motion / mlen;
- real_t min, max;
+ real_t min = 0.0, max = 0.0;
p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
// Did it move enough in this direction to even attempt raycast?
diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp
index 9fe0e3eb84..ca76a819ec 100644
--- a/servers/physics_3d/godot_collision_solver_3d.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d.cpp
@@ -48,7 +48,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s
static const int max_supports = 16;
Vector3 supports[max_supports];
int support_count;
- GodotShape3D::FeatureType support_type;
+ GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT;
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
if (support_type == GodotShape3D::FEATURE_CIRCLE) {
@@ -338,7 +338,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const
real_t axis_scale = 1.0 / axis.length();
axis *= axis_scale;
- real_t smin, smax;
+ real_t smin = 0.0, smax = 0.0;
p_shape_A->project_range(axis, rel_transform, smin, smax);
smin -= p_margin_A;
smax += p_margin_A;
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 56e644b57b..96253cb452 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -634,7 +634,7 @@ public:
axis = Vector3(0.0, 1.0, 0.0);
}
- real_t min_A, max_A, min_B, max_B;
+ real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
shape_A->project_range(axis, *transform_A, min_A, max_A);
shape_B->project_range(axis, *transform_B, min_B, max_B);
@@ -964,8 +964,8 @@ static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Tran
// edges of B
for (int i = 0; i < edge_count; i++) {
- Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]);
- Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]);
+ Vector3 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]);
+ Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]);
Vector3 v3 = p_transform_a.origin;
Vector3 n1 = v2 - v1;
@@ -1404,7 +1404,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo
Vector3 e1 = p_transform_a.basis.get_column(i);
for (int j = 0; j < edge_count; j++) {
- Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
+ Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
Vector3 axis = e1.cross(e2).normalized();
@@ -1460,8 +1460,8 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo
}
for (int e = 0; e < edge_count; e++) {
- Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]);
- Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]);
+ Vector3 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]);
+ Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]);
Vector3 n = (p2 - p1);
if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
@@ -1771,7 +1771,7 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra
for (int i = 0; i < edge_count; i++) {
// cylinder
- Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]);
+ Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]);
Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized();
if (!separator.test_axis(axis)) {
@@ -1789,8 +1789,8 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra
Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
for (int j = 0; j < edge_count; j++) {
- Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]);
- Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
+ Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]);
+ Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
Vector3 axis = n1.cross(n2).cross(n2).normalized();
@@ -2075,6 +2075,16 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D
separator.generate_contacts();
}
+static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) {
+ // Test if arcs AB and CD intersect on the unit sphere
+ real_t CBA = C.dot(B_x_A);
+ real_t DBA = D.dot(B_x_A);
+ real_t ADC = A.dot(D_x_C);
+ real_t BDC = B.dot(D_x_C);
+
+ return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f);
+}
+
template <bool withMargin>
static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
@@ -2129,16 +2139,27 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
}
// A<->B edges
+
for (int i = 0; i < edge_count_A; i++) {
- Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
+ Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]);
+ Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]);
+ Vector3 e1 = q1 - p1;
+ Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized();
+ Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized();
for (int j = 0; j < edge_count_B; j++) {
- Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]);
+ Vector3 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]);
+ Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]);
+ Vector3 e2 = q2 - p2;
+ Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized();
+ Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized();
- Vector3 axis = e1.cross(e2).normalized();
+ if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) {
+ Vector3 axis = e1.cross(e2).normalized();
- if (!separator.test_axis(axis)) {
- return;
+ if (!separator.test_axis(axis)) {
+ return;
+ }
}
}
}
@@ -2157,8 +2178,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
//edge-vertex (shell)
for (int i = 0; i < edge_count_A; i++) {
- Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]);
- Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
+ Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]);
+ Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]);
Vector3 n = (e2 - e1);
for (int j = 0; j < vertex_count_B; j++) {
@@ -2171,8 +2192,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
}
for (int i = 0; i < edge_count_B; i++) {
- Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]);
- Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]);
+ Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]);
+ Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]);
Vector3 n = (e2 - e1);
for (int j = 0; j < vertex_count_A; j++) {
@@ -2231,7 +2252,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
// A<->B edges
for (int i = 0; i < edge_count; i++) {
- Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]);
+ Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]);
for (int j = 0; j < 3; j++) {
Vector3 e2 = vertex[j] - vertex[(j + 1) % 3];
@@ -2266,8 +2287,8 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
//edge-vertex (shell)
for (int i = 0; i < edge_count; i++) {
- Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]);
- Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]);
+ Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]);
+ Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]);
Vector3 n = (e2 - e1);
for (int j = 0; j < 3; j++) {
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index 5574be20b7..1443cd166b 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -411,9 +411,9 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *
}
bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
- AABB aabb(-half_extents, half_extents * 2.0);
+ AABB aabb_ext(-half_extents, half_extents * 2.0);
- return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
+ return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal);
}
bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const {
@@ -817,48 +817,78 @@ GodotCylinderShape3D::GodotCylinderShape3D() {}
/********** CONVEX POLYGON *************/
void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
- int vertex_count = mesh.vertices.size();
+ uint32_t vertex_count = mesh.vertices.size();
if (vertex_count == 0) {
return;
}
const Vector3 *vrts = &mesh.vertices[0];
- for (int i = 0; i < vertex_count; i++) {
- real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+ if (vertex_count > 3 * extreme_vertices.size()) {
+ // For a large mesh, two calls to get_support() is faster than a full
+ // scan over all vertices.
- if (i == 0 || d > r_max) {
- r_max = d;
- }
- if (i == 0 || d < r_min) {
- r_min = d;
+ Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
+ r_min = p_normal.dot(p_transform.xform(get_support(-n)));
+ r_max = p_normal.dot(p_transform.xform(get_support(n)));
+ } else {
+ for (uint32_t i = 0; i < vertex_count; i++) {
+ real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+
+ if (i == 0 || d > r_max) {
+ r_max = d;
+ }
+ if (i == 0 || d < r_min) {
+ r_min = d;
+ }
}
}
}
Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
- Vector3 n = p_normal;
-
- int vert_support_idx = -1;
- real_t support_max = 0;
-
- int vertex_count = mesh.vertices.size();
- if (vertex_count == 0) {
+ if (mesh.vertices.size() == 0) {
return Vector3();
}
- const Vector3 *vrts = &mesh.vertices[0];
-
- for (int i = 0; i < vertex_count; i++) {
- real_t d = n.dot(vrts[i]);
+ // Find an initial guess for the support vertex by checking the ones we
+ // found in _setup().
- if (i == 0 || d > support_max) {
- support_max = d;
- vert_support_idx = i;
+ int best_vertex = -1;
+ real_t max_support = 0.0;
+ for (uint32_t i = 0; i < extreme_vertices.size(); i++) {
+ real_t s = p_normal.dot(mesh.vertices[extreme_vertices[i]]);
+ if (best_vertex == -1 || s > max_support) {
+ best_vertex = extreme_vertices[i];
+ max_support = s;
}
}
+ if (extreme_vertices.size() == mesh.vertices.size()) {
+ // We've already checked every vertex, so we can return now.
+ return mesh.vertices[best_vertex];
+ }
- return vrts[vert_support_idx];
+ // Move along the surface until we reach the true support vertex.
+
+ int last_vertex = -1;
+ while (true) {
+ int next_vertex = -1;
+ for (uint32_t i = 0; i < vertex_neighbors[best_vertex].size(); i++) {
+ int vert = vertex_neighbors[best_vertex][i];
+ if (vert != last_vertex) {
+ real_t s = p_normal.dot(mesh.vertices[vert]);
+ if (s > max_support) {
+ next_vertex = vert;
+ max_support = s;
+ break;
+ }
+ }
+ }
+ if (next_vertex == -1) {
+ return mesh.vertices[best_vertex];
+ }
+ last_vertex = best_vertex;
+ best_vertex = next_vertex;
+ }
}
void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
@@ -915,13 +945,13 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max,
}
for (int i = 0; i < ec; i++) {
- real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
+ real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal);
dot = ABS(dot);
- if (dot < edge_support_threshold && (edges[i].a == vtx || edges[i].b == vtx)) {
+ if (dot < edge_support_threshold && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) {
r_amount = 2;
r_type = FEATURE_EDGE;
- r_supports[0] = vertices[edges[i].a];
- r_supports[1] = vertices[edges[i].b];
+ r_supports[0] = vertices[edges[i].vertex_a];
+ r_supports[1] = vertices[edges[i].vertex_b];
return;
}
}
@@ -1025,8 +1055,8 @@ Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point)
int ec = mesh.edges.size();
for (int i = 0; i < ec; i++) {
Vector3 s[2] = {
- vertices[edges[i].a],
- vertices[edges[i].b]
+ vertices[edges[i].vertex_a],
+ vertices[edges[i].vertex_b]
};
Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s);
@@ -1058,7 +1088,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
AABB _aabb;
- for (int i = 0; i < mesh.vertices.size(); i++) {
+ for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
if (i == 0) {
_aabb.position = mesh.vertices[i];
} else {
@@ -1067,6 +1097,43 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
}
configure(_aabb);
+
+ // Pre-compute the extreme vertices in 26 directions. This will be used
+ // to speed up get_support() by letting us quickly get a good guess for
+ // the support vertex.
+
+ for (int x = -1; x < 2; x++) {
+ for (int y = -1; y < 2; y++) {
+ for (int z = -1; z < 2; z++) {
+ if (x != 0 || y != 0 || z != 0) {
+ Vector3 dir(x, y, z);
+ dir.normalize();
+ real_t max_support = 0.0;
+ int best_vertex = -1;
+ for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
+ real_t s = dir.dot(mesh.vertices[i]);
+ if (best_vertex == -1 || s > max_support) {
+ best_vertex = i;
+ max_support = s;
+ }
+ }
+ if (extreme_vertices.find(best_vertex) == -1)
+ extreme_vertices.push_back(best_vertex);
+ }
+ }
+ }
+ }
+
+ // Record all the neighbors of each vertex. This is used in get_support().
+
+ if (extreme_vertices.size() < mesh.vertices.size()) {
+ vertex_neighbors.resize(mesh.vertices.size());
+ for (uint32_t i = 0; i < mesh.edges.size(); i++) {
+ Geometry3D::MeshData::Edge &edge = mesh.edges[i];
+ vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b);
+ vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a);
+ }
+ }
}
void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
@@ -1074,7 +1141,12 @@ void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
}
Variant GodotConvexPolygonShape3D::get_data() const {
- return mesh.vertices;
+ Vector<Vector3> vertices;
+ vertices.resize(mesh.vertices.size());
+ for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
+ vertices.write[i] = mesh.vertices[i];
+ }
+ return vertices;
}
GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() {
@@ -1259,14 +1331,14 @@ Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const {
}
void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
- const BVH *bvh = &p_params->bvh[p_idx];
+ const BVH *params_bvh = &p_params->bvh[p_idx];
- if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
+ if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
return;
}
- if (bvh->face_index >= 0) {
- const Face *f = &p_params->faces[bvh->face_index];
+ if (params_bvh->face_index >= 0) {
+ const Face *f = &p_params->faces[params_bvh->face_index];
GodotFaceShape3D *face = p_params->face;
face->normal = f->normal;
face->vertex[0] = p_params->vertices[f->indices[0]];
@@ -1285,11 +1357,11 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_
}
}
} else {
- if (bvh->left >= 0) {
- _cull_segment(bvh->left, p_params);
+ if (params_bvh->left >= 0) {
+ _cull_segment(params_bvh->left, p_params);
}
- if (bvh->right >= 0) {
- _cull_segment(bvh->right, p_params);
+ if (params_bvh->right >= 0) {
+ _cull_segment(params_bvh->right, p_params);
}
}
}
@@ -1339,14 +1411,14 @@ Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point)
}
bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
- const BVH *bvh = &p_params->bvh[p_idx];
+ const BVH *params_bvh = &p_params->bvh[p_idx];
- if (!p_params->aabb.intersects(bvh->aabb)) {
+ if (!p_params->aabb.intersects(params_bvh->aabb)) {
return false;
}
- if (bvh->face_index >= 0) {
- const Face *f = &p_params->faces[bvh->face_index];
+ if (params_bvh->face_index >= 0) {
+ const Face *f = &p_params->faces[params_bvh->face_index];
GodotFaceShape3D *face = p_params->face;
face->normal = f->normal;
face->vertex[0] = p_params->vertices[f->indices[0]];
@@ -1356,14 +1428,14 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
return true;
}
} else {
- if (bvh->left >= 0) {
- if (_cull(bvh->left, p_params)) {
+ if (params_bvh->left >= 0) {
+ if (_cull(params_bvh->left, p_params)) {
return true;
}
}
- if (bvh->right >= 0) {
- if (_cull(bvh->right, p_params)) {
+ if (params_bvh->right >= 0) {
+ if (_cull(params_bvh->right, p_params)) {
return true;
}
}
@@ -1919,14 +1991,14 @@ Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) cons
}
void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
- const AABB &aabb = get_aabb();
+ const AABB &shape_aabb = get_aabb();
- Vector3 pos_local = aabb.position + local_origin;
+ Vector3 pos_local = shape_aabb.position + local_origin;
Vector3 clamped_point(p_point);
- clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x);
- clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y);
- clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z);
+ clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x);
+ clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y);
+ clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + shape_aabb.size.z);
r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
@@ -2070,19 +2142,19 @@ void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width,
depth = p_depth;
// Initialize aabb.
- AABB aabb;
- aabb.position = Vector3(0.0, p_min_height, 0.0);
- aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
+ AABB aabb_new;
+ aabb_new.position = Vector3(0.0, p_min_height, 0.0);
+ aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
// Initialize origin as the aabb center.
- local_origin = aabb.position + 0.5 * aabb.size;
+ local_origin = aabb_new.position + 0.5 * aabb_new.size;
local_origin.y = 0.0;
- aabb.position -= local_origin;
+ aabb_new.position -= local_origin;
_build_accelerator();
- configure(aabb);
+ configure(aabb_new);
}
void GodotHeightMapShape3D::set_data(const Variant &p_data) {
@@ -2093,11 +2165,11 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) {
ERR_FAIL_COND(!d.has("depth"));
ERR_FAIL_COND(!d.has("heights"));
- int width = d["width"];
- int depth = d["depth"];
+ int width_new = d["width"];
+ int depth_new = d["depth"];
- ERR_FAIL_COND(width <= 0.0);
- ERR_FAIL_COND(depth <= 0.0);
+ ERR_FAIL_COND(width_new <= 0.0);
+ ERR_FAIL_COND(depth_new <= 0.0);
Variant heights_variant = d["heights"];
Vector<real_t> heights_buffer;
@@ -2151,10 +2223,10 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) {
ERR_FAIL_COND(min_height > max_height);
- ERR_FAIL_COND(heights_buffer.size() != (width * depth));
+ ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new));
// If specified, min and max height will be used as precomputed values.
- _setup(heights_buffer, width, depth, min_height, max_height);
+ _setup(heights_buffer, width_new, depth_new, min_height, max_height);
}
Variant GodotHeightMapShape3D::get_data() const {
@@ -2162,9 +2234,9 @@ Variant GodotHeightMapShape3D::get_data() const {
d["width"] = width;
d["depth"] = depth;
- const AABB &aabb = get_aabb();
- d["min_height"] = aabb.position.y;
- d["max_height"] = aabb.position.y + aabb.size.y;
+ const AABB &shape_aabb = get_aabb();
+ d["min_height"] = shape_aabb.position.y;
+ d["max_height"] = shape_aabb.position.y + shape_aabb.size.y;
d["heights"] = heights;
diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h
index 1fc8f7c711..dc8e34e2bc 100644
--- a/servers/physics_3d/godot_shape_3d.h
+++ b/servers/physics_3d/godot_shape_3d.h
@@ -277,6 +277,8 @@ public:
struct GodotConvexPolygonShape3D : public GodotShape3D {
Geometry3D::MeshData mesh;
+ LocalVector<int> extreme_vertices;
+ LocalVector<LocalVector<int>> vertex_neighbors;
void _setup(const Vector<Vector3> &p_vertices);
diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp
index 173843072a..77110c5fbc 100644
--- a/servers/physics_3d/godot_soft_body_3d.cpp
+++ b/servers/physics_3d/godot_soft_body_3d.cpp
@@ -722,7 +722,14 @@ void GodotSoftBody3D::reoptimize_link_order() {
const int reop_not_dependent = -1;
const int reop_node_complete = -2;
- uint32_t i, link_count = links.size(), node_count = nodes.size();
+ uint32_t link_count = links.size();
+ uint32_t node_count = nodes.size();
+
+ if (link_count < 1 || node_count < 2) {
+ return;
+ }
+
+ uint32_t i;
Link *lr;
int ar, br;
Node *node0 = &(nodes[0]);
generated by cgit v1.2.3 (git 2.25.1) at 2026-01-05 20:10:41 +0000