summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/godot_collision_solver_3d_sat.cpp44
-rw-r--r--servers/physics_3d/godot_shape_3d.cpp33
-rw-r--r--servers/physics_3d/godot_soft_body_3d.cpp93
3 files changed, 63 insertions, 107 deletions
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 66d1811abb..2cb29b3dd0 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -827,9 +827,9 @@ static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the two spheres
analytic_sphere_collision<withMargin>(
p_transform_a.origin,
- sphere_A->get_radius(),
+ sphere_A->get_radius() * p_transform_a.basis[0].length(),
p_transform_b.origin,
- sphere_B->get_radius(),
+ sphere_B->get_radius() * p_transform_b.basis[0].length(),
p_collector,
p_margin_a,
p_margin_b);
@@ -842,7 +842,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
// Find the point on the box nearest to the center of the sphere.
- Vector3 center = p_transform_b.xform_inv(p_transform_a.origin);
+ Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 extents = box_B->get_half_extents();
Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x),
MIN(MAX(center.y, -extents.y), extents.y),
@@ -853,7 +853,8 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length();
- if (length > sphere_A->get_radius() + p_margin_a + p_margin_b) {
+ real_t radius = sphere_A->get_radius() * p_transform_a.basis[0].length();
+ if (length > radius + p_margin_a + p_margin_b) {
return;
}
p_collector->collided = true;
@@ -867,7 +868,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
} else {
axis = delta / length;
}
- Vector3 point_a = p_transform_a.origin + (sphere_A->get_radius() + p_margin_a) * axis;
+ Vector3 point_a = p_transform_a.origin + (radius + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis);
}
@@ -877,11 +878,12 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
- real_t capsule_B_radius = capsule_B->get_radius();
+ real_t scale_A = p_transform_a.basis[0].length();
+ real_t scale_B = p_transform_b.basis[0].length();
// Construct the capsule segment (ball-center to ball-center)
Vector3 capsule_segment[2];
- Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius);
+ Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
capsule_segment[0] = p_transform_b.origin + capsule_axis;
capsule_segment[1] = p_transform_b.origin - capsule_axis;
@@ -891,9 +893,9 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule
analytic_sphere_collision<withMargin>(
p_transform_a.origin,
- sphere_A->get_radius(),
+ sphere_A->get_radius() * scale_A,
capsule_closest,
- capsule_B_radius,
+ capsule_B->get_radius() * scale_B,
p_collector,
p_margin_a,
p_margin_b);
@@ -903,12 +905,12 @@ template <bool withMargin>
static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
// Find the point on the cylinder nearest to the center of the sphere.
- Vector3 center = p_transform_b.xform_inv(p_transform_a.origin);
+ Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 nearest = center;
- real_t radius = p_radius_b;
+ real_t scale_A = p_transform_a.basis[0].length();
real_t r = Math::sqrt(center.x * center.x + center.z * center.z);
- if (r > radius) {
- real_t scale = radius / r;
+ if (r > p_radius_b) {
+ real_t scale = p_radius_b / r;
nearest.x *= scale;
nearest.z *= scale;
}
@@ -920,7 +922,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length();
- if (length > p_radius_a + p_margin_a + p_margin_b) {
+ if (length > p_radius_a * scale_A + p_margin_a + p_margin_b) {
return;
}
p_collector->collided = true;
@@ -934,7 +936,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
} else {
axis = delta / length;
}
- Vector3 point_a = p_transform_a.origin + (p_radius_a + p_margin_a) * axis;
+ Vector3 point_a = p_transform_a.origin + (p_radius_a * scale_A + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis);
}
@@ -1632,14 +1634,14 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
- real_t capsule_A_radius = capsule_A->get_radius();
- real_t capsule_B_radius = capsule_B->get_radius();
+ real_t scale_A = p_transform_a.basis[0].length();
+ real_t scale_B = p_transform_b.basis[0].length();
// Get the closest points between the capsule segments
Vector3 capsule_A_closest;
Vector3 capsule_B_closest;
- Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A_radius);
- Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius);
+ Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
+ Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
Geometry3D::get_closest_points_between_segments(
p_transform_a.origin + capsule_A_axis,
p_transform_a.origin - capsule_A_axis,
@@ -1651,9 +1653,9 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
// Perform the analytic collision between the two closest capsule spheres
analytic_sphere_collision<withMargin>(
capsule_A_closest,
- capsule_A_radius,
+ capsule_A->get_radius() * scale_A,
capsule_B_closest,
- capsule_B_radius,
+ capsule_B->get_radius() * scale_B,
p_collector,
p_margin_a,
p_margin_b);
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index 8aacfa929f..300dca4e08 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -852,17 +852,12 @@ Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
// Get the array of vertices
const Vector3 *const vertices_array = mesh.vertices.ptr();
- // Get the array of extreme vertices
- const int *const extreme_array = extreme_vertices.ptr();
- const uint32_t extreme_size = extreme_vertices.size();
-
- // Start with an initial assumption of the first extreme vertex
- int best_vertex = extreme_array[0];
+ // Start with an initial assumption of the first extreme vertex.
+ int best_vertex = extreme_vertices[0];
real_t max_support = p_normal.dot(vertices_array[best_vertex]);
- // Check the remaining extreme vertices for a better vertex
- for (uint32_t i = 0; i < extreme_size; ++i) {
- int vert = extreme_array[i];
+ // Check the remaining extreme vertices for a better vertex.
+ for (const int &vert : extreme_vertices) {
real_t s = p_normal.dot(vertices_array[vert]);
if (s > max_support) {
best_vertex = vert;
@@ -870,27 +865,18 @@ Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
}
}
- // If we checked all vertices in the mesh then we're done
- if (extreme_size == mesh.vertices.size()) {
+ // If we checked all vertices in the mesh then we're done.
+ if (extreme_vertices.size() == mesh.vertices.size()) {
return vertices_array[best_vertex];
}
- // Get the array of neighbor arrays for each vertex
- const LocalVector<int> *const vertex_neighbors_array = vertex_neighbors.ptr();
-
// Move along the surface until we reach the true support vertex.
int last_vertex = -1;
while (true) {
int next_vertex = -1;
- // Get the array of neighbors around the best vertex
- const LocalVector<int> &neighbors = vertex_neighbors_array[best_vertex];
- const int *const neighbors_array = neighbors.ptr();
- const uint32_t neighbors_size = neighbors.size();
-
- // Iterate over all the neighbors checking for a better vertex
- for (uint32_t i = 0; i < neighbors_size; ++i) {
- int vert = neighbors_array[i];
+ // Iterate over all the neighbors checking for a better vertex.
+ for (const int &vert : vertex_neighbors[best_vertex]) {
if (vert != last_vertex) {
real_t s = p_normal.dot(vertices_array[vert]);
if (s > max_support) {
@@ -1149,8 +1135,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
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];
+ for (Geometry3D::MeshData::Edge &edge : mesh.edges) {
vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b);
vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a);
}
diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp
index 871017a5e7..1820a29553 100644
--- a/servers/physics_3d/godot_soft_body_3d.cpp
+++ b/servers/physics_3d/godot_soft_body_3d.cpp
@@ -167,14 +167,11 @@ void GodotSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHand
}
void GodotSoftBody3D::update_normals_and_centroids() {
- uint32_t i, ni;
-
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- nodes[i].n = Vector3();
+ for (Node &node : nodes) {
+ node.n = Vector3();
}
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- Face &face = faces[i];
+ for (Face &face : faces) {
const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x);
face.n[0]->n += n;
face.n[1]->n += n;
@@ -184,8 +181,7 @@ void GodotSoftBody3D::update_normals_and_centroids() {
face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x);
}
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
real_t len = node.n.length();
if (len > CMP_EPSILON) {
node.n /= len;
@@ -235,9 +231,7 @@ void GodotSoftBody3D::update_area() {
int i, ni;
// Face area.
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- Face &face = faces[i];
-
+ for (Face &face : faces) {
const Vector3 &x0 = face.n[0]->x;
const Vector3 &x1 = face.n[1]->x;
const Vector3 &x2 = face.n[2]->x;
@@ -255,12 +249,11 @@ void GodotSoftBody3D::update_area() {
memset(counts.ptr(), 0, counts.size() * sizeof(int));
}
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- nodes[i].area = 0.0;
+ for (Node &node : nodes) {
+ node.area = 0.0;
}
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- const Face &face = faces[i];
+ for (const Face &face : faces) {
for (int j = 0; j < 3; ++j) {
const int index = (int)(face.n[j] - &nodes[0]);
counts[index]++;
@@ -278,8 +271,7 @@ void GodotSoftBody3D::update_area() {
}
void GodotSoftBody3D::reset_link_rest_lengths() {
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.rl = (link.n[0]->x - link.n[1]->x).length();
link.c1 = link.rl * link.rl;
}
@@ -287,8 +279,7 @@ void GodotSoftBody3D::reset_link_rest_lengths() {
void GodotSoftBody3D::update_link_constants() {
real_t inv_linear_stiffness = 1.0 / linear_stiffness;
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness;
}
}
@@ -619,9 +610,9 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
}
}
}
- for (i = 0; i < links.size(); ++i) {
- const int ia = (int)(links[i].n[0] - &nodes[0]);
- const int ib = (int)(links[i].n[1] - &nodes[0]);
+ for (Link &link : links) {
+ const int ia = (int)(link.n[0] - &nodes[0]);
+ const int ib = (int)(link.n[1] - &nodes[0]);
int idx = ib * n + ia;
int idx_inv = ia * n + ib;
adj[idx] = 1;
@@ -635,9 +626,9 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
// Build node links.
node_links.resize(nodes.size());
- for (i = 0; i < links.size(); ++i) {
- const int ia = (int)(links[i].n[0] - &nodes[0]);
- const int ib = (int)(links[i].n[1] - &nodes[0]);
+ for (Link &link : links) {
+ const int ia = (int)(link.n[0] - &nodes[0]);
+ const int ib = (int)(link.n[1] - &nodes[0]);
if (node_links[ia].find(ib) == -1) {
node_links[ia].push_back(ib);
}
@@ -649,8 +640,7 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
for (uint32_t ii = 0; ii < node_links.size(); ii++) {
for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) {
int k = node_links[ii][jj];
- for (uint32_t kk = 0; kk < node_links[k].size(); kk++) {
- int l = node_links[k][kk];
+ for (const int &l : node_links[k]) {
if ((int)ii != l) {
int idx_ik = k * n + ii;
int idx_kj = l * n + k;
@@ -916,8 +906,7 @@ void GodotSoftBody3D::set_drag_coefficient(real_t p_val) {
}
void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) {
- for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
if (node.im > 0) {
node.v += p_velocity;
}
@@ -929,26 +918,22 @@ void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_area
return;
}
- uint32_t i, ni;
int32_t j;
real_t volume = 0.0;
const Vector3 &org = nodes[0].x;
// Iterate over faces (try not to iterate elsewhere if possible).
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- const Face &face = faces[i];
-
+ for (const Face &face : faces) {
Vector3 wind_force(0, 0, 0);
// Compute volume.
volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
// Compute nodal forces from area winds.
- int wind_area_count = p_wind_areas.size();
- if (wind_area_count > 0) {
- for (j = 0; j < wind_area_count; j++) {
- wind_force += _compute_area_windforce(p_wind_areas[j], &face);
+ if (!p_wind_areas.is_empty()) {
+ for (const GodotArea3D *area : p_wind_areas) {
+ wind_force += _compute_area_windforce(area, &face);
}
for (j = 0; j < 3; j++) {
@@ -962,8 +947,7 @@ void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_area
// Apply nodal pressure forces.
if (pressure_coefficient > CMP_EPSILON) {
real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
if (node.im > 0) {
node.f += node.n * (node.area * ivolumetp);
}
@@ -1048,9 +1032,7 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
real_t clamp_delta_v = max_displacement * inv_delta;
// Integrate.
- uint32_t i, ni;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
node.q = node.x;
Vector3 delta_v = node.f * node.im * p_delta;
for (int c = 0; c < 3; c++) {
@@ -1065,9 +1047,7 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
update_bounds();
// Node tree update.
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- const Node &node = nodes[i];
-
+ for (const Node &node : nodes) {
AABB node_aabb(node.x, Vector3());
node_aabb.expand_to(node.x + node.v * p_delta);
node_aabb.grow_by(collision_margin);
@@ -1088,17 +1068,13 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
void GodotSoftBody3D::solve_constraints(real_t p_delta) {
const real_t inv_delta = 1.0 / p_delta;
- uint32_t i, ni;
-
- for (i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.c3 = link.n[1]->q - link.n[0]->q;
link.c2 = 1 / (link.c3.length_squared() * link.c0);
}
// Solve velocities.
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
node.x = node.q + node.v * p_delta;
}
@@ -1108,9 +1084,7 @@ void GodotSoftBody3D::solve_constraints(real_t p_delta) {
solve_links(1.0, ti);
}
const real_t vc = (1.0 - damping_coefficient) * inv_delta;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
-
+ for (Node &node : nodes) {
node.x += node.bv * p_delta;
node.bv = Vector3();
@@ -1123,8 +1097,7 @@ void GodotSoftBody3D::solve_constraints(real_t p_delta) {
}
void GodotSoftBody3D::solve_links(real_t kst, real_t ti) {
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
if (link.c0 > 0) {
Node &node_a = *link.n[0];
Node &node_b = *link.n[1];
@@ -1183,9 +1156,7 @@ void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, Godo
void GodotSoftBody3D::initialize_face_tree() {
face_tree.clear();
- for (uint32_t i = 0; i < faces.size(); ++i) {
- Face &face = faces[i];
-
+ for (Face &face : faces) {
AABB face_aabb;
face_aabb.position = face.n[0]->x;
@@ -1199,9 +1170,7 @@ void GodotSoftBody3D::initialize_face_tree() {
}
void GodotSoftBody3D::update_face_tree(real_t p_delta) {
- for (uint32_t i = 0; i < faces.size(); ++i) {
- const Face &face = faces[i];
-
+ for (const Face &face : faces) {
AABB face_aabb;
const Node *node0 = face.n[0];
generated by cgit v1.2.3 (git 2.25.1) at 2025-12-29 09:19:16 +0000