summaryrefslogtreecommitdiff
path: root/servers/physics_3d/shape_3d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/shape_3d_sw.cpp')
-rw-r--r--servers/physics_3d/shape_3d_sw.cpp100
1 files changed, 65 insertions, 35 deletions
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index a44244f7d5..eb0e87cec0 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -105,8 +105,9 @@ Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const {
bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
- if (inters)
+ if (inters) {
r_normal = plane.normal;
+ }
return inters;
}
@@ -159,10 +160,11 @@ void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tra
}
Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const {
- if (p_normal.z > 0)
+ if (p_normal.z > 0) {
return Vector3(0, 0, length);
- else
+ } else {
return Vector3(0, 0, 0);
+ }
}
void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
@@ -260,8 +262,9 @@ bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const {
Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
Vector3 p = p_point;
float l = p.length();
- if (l < radius)
+ if (l < radius) {
return p_point;
+ }
return (p / l) * radius;
}
@@ -418,11 +421,13 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
}
}
- if (!outside)
+ if (!outside) {
return p_point; //it's inside, don't do anything else
+ }
- if (outside == 1) //if only above one plane, this plane clearly wins
+ if (outside == 1) { //if only above one plane, this plane clearly wins
return min_point;
+ }
//check segments
float min_distance = 1e20;
@@ -597,8 +602,9 @@ Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
Vector3 p = Geometry::get_closest_point_to_segment(p_point, s);
- if (p.distance_to(p_point) < radius)
+ if (p.distance_to(p_point) < radius) {
return p_point;
+ }
return p + (p_point - p).normalized() * radius;
}
@@ -641,18 +647,21 @@ CapsuleShape3DSW::CapsuleShape3DSW() {
void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
int vertex_count = mesh.vertices.size();
- if (vertex_count == 0)
+ 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 (i == 0 || d > r_max)
+ if (i == 0 || d > r_max) {
r_max = d;
- if (i == 0 || d < r_min)
+ }
+ if (i == 0 || d < r_min) {
r_min = d;
+ }
}
}
@@ -663,8 +672,9 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
real_t support_max = 0;
int vertex_count = mesh.vertices.size();
- if (vertex_count == 0)
+ if (vertex_count == 0) {
return Vector3();
+ }
const Vector3 *vrts = &mesh.vertices[0];
@@ -716,8 +726,9 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
}
}
- if (!valid)
+ if (!valid) {
continue;
+ }
int m = MIN(p_max, ic);
for (int j = 0; j < m; j++) {
@@ -754,8 +765,9 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec
bool col = false;
for (int i = 0; i < fc; i++) {
- if (faces[i].plane.normal.dot(n) > 0)
+ if (faces[i].plane.normal.dot(n) > 0) {
continue; //opposing face
+ }
int ic = faces[i].indices.size();
const int *ind = faces[i].indices.ptr();
@@ -785,8 +797,9 @@ bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
int fc = mesh.faces.size();
for (int i = 0; i < fc; i++) {
- if (faces[i].plane.distance_to(p_point) >= 0)
+ if (faces[i].plane.distance_to(p_point) >= 0) {
return false;
+ }
}
return true;
@@ -799,8 +812,9 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con
bool all_inside = true;
for (int i = 0; i < fc; i++) {
- if (!faces[i].plane.is_point_over(p_point))
+ if (!faces[i].plane.is_point_over(p_point)) {
continue;
+ }
all_inside = false;
bool is_inside = true;
@@ -861,16 +875,18 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
Error err = QuickHull::build(p_vertices, mesh);
- if (err != OK)
+ if (err != OK) {
ERR_PRINT("Failed to build QuickHull");
+ }
AABB _aabb;
for (int i = 0; i < mesh.vertices.size(); i++) {
- if (i == 0)
+ if (i == 0) {
_aabb.position = mesh.vertices[i];
- else
+ } else {
_aabb.expand_to(mesh.vertices[i]);
+ }
}
configure(_aabb);
@@ -894,11 +910,13 @@ void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tr
Vector3 v = p_transform.xform(vertex[i]);
real_t d = p_normal.dot(v);
- if (i == 0 || d > r_max)
+ if (i == 0 || d > r_max) {
r_max = d;
+ }
- if (i == 0 || d < r_min)
+ if (i == 0 || d < r_min) {
r_min = d;
+ }
}
}
@@ -948,8 +966,9 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_
for (int i = 0; i < 3; i++) {
int nx = (i + 1) % 3;
- if (i != vert_support_idx && nx != vert_support_idx)
+ if (i != vert_support_idx && nx != vert_support_idx) {
continue;
+ }
// check if edge is valid as a support
real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n);
@@ -1021,17 +1040,20 @@ void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Trans
for (int i = 0; i < count; i++) {
real_t d = p_normal.dot(p_transform.xform(vptr[i]));
- if (i == 0 || d > r_max)
+ if (i == 0 || d > r_max) {
r_max = d;
- if (i == 0 || d < r_min)
+ }
+ if (i == 0 || d < r_min) {
r_min = d;
+ }
}
}
Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const {
int count = vertices.size();
- if (count == 0)
+ if (count == 0) {
return Vector3();
+ }
const Vector3 *vptr = vertices.ptr();
@@ -1091,16 +1113,19 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par
}
} else {
- if (bvh->left >= 0)
+ if (bvh->left >= 0) {
_cull_segment(bvh->left, p_params);
- if (bvh->right >= 0)
+ }
+ if (bvh->right >= 0) {
_cull_segment(bvh->right, p_params);
+ }
}
}
bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
- if (faces.size() == 0)
+ if (faces.size() == 0) {
return false;
+ }
// unlock data
const Face *fr = faces.ptr();
@@ -1141,8 +1166,9 @@ Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) co
void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
const BVH *bvh = &p_params->bvh[p_idx];
- if (!p_params->aabb.intersects(bvh->aabb))
+ if (!p_params->aabb.intersects(bvh->aabb)) {
return;
+ }
if (bvh->face_index >= 0) {
const Face *f = &p_params->faces[bvh->face_index];
@@ -1166,8 +1192,9 @@ void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
// make matrix local to concave
- if (faces.size() == 0)
+ if (faces.size() == 0) {
return;
+ }
AABB local_aabb = p_local_aabb;
@@ -1250,10 +1277,11 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz
AABB aabb;
for (int i = 0; i < p_size; i++) {
- if (i == 0)
+ if (i == 0) {
aabb = p_elements[i].aabb;
- else
+ } else {
aabb.merge_with(p_elements[i].aabb);
+ }
}
bvh->aabb = aabb;
switch (aabb.get_longest_axis_index()) {
@@ -1345,10 +1373,11 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) {
verticesw[i * 3 + 0] = face.vertex[0];
verticesw[i * 3 + 1] = face.vertex[1];
verticesw[i * 3 + 2] = face.vertex[2];
- if (i == 0)
+ if (i == 0) {
_aabb = bvh_arrayw[i].aabb;
- else
+ } else {
_aabb.merge_with(bvh_arrayw[i].aabb);
+ }
}
int count = 0;
@@ -1443,10 +1472,11 @@ void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_dep
real_t h = r[i * width + j];
Vector3 pos(j * cell_size, h, i * cell_size);
- if (i == 0 || j == 0)
+ if (i == 0 || j == 0) {
aabb.position = pos;
- else
+ } else {
aabb.expand_to(pos);
+ }
}
}