summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--editor/plugins/path_3d_editor_plugin.cpp7
-rw-r--r--scene/resources/curve.cpp52
-rw-r--r--scene/resources/curve.h2
-rw-r--r--servers/physics_2d/godot_body_pair_2d.cpp23
-rw-r--r--servers/physics_3d/godot_body_pair_3d.cpp29
5 files changed, 82 insertions, 31 deletions
diff --git a/editor/plugins/path_3d_editor_plugin.cpp b/editor/plugins/path_3d_editor_plugin.cpp
index 63ca78d6c0..7a9e50e482 100644
--- a/editor/plugins/path_3d_editor_plugin.cpp
+++ b/editor/plugins/path_3d_editor_plugin.cpp
@@ -274,13 +274,10 @@ void Path3DGizmo::redraw() {
// Fish Bone.
v3p.push_back(p1);
- v3p.push_back(p1 + (side - forward) * 0.06);
+ v3p.push_back(p1 + (side - forward + up * 0.3) * 0.06);
v3p.push_back(p1);
- v3p.push_back(p1 + (-side - forward) * 0.06);
-
- v3p.push_back(p1);
- v3p.push_back(p1 + up * 0.03);
+ v3p.push_back(p1 + (-side - forward + up * 0.3) * 0.06);
}
add_lines(v3p, path_material);
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index bc2149a8c6..b0a63bb7fa 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -790,6 +790,19 @@ void Curve2D::_bake_segment2d_even_length(RBMap<real_t, Vector2> &r_bake, real_t
}
}
+Vector2 Curve2D::_calculate_tangent(const Vector2 &p_begin, const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) {
+ // Handle corner cases.
+ if (Math::is_zero_approx(p_t - 0.0f) && p_control_1.is_equal_approx(p_begin)) {
+ return (p_end - p_begin).normalized();
+ }
+
+ if (Math::is_zero_approx(p_t - 1.0f) && p_control_2.is_equal_approx(p_end)) {
+ return (p_end - p_begin).normalized();
+ }
+
+ return p_begin.bezier_derivative(p_control_1, p_control_2, p_end, p_t).normalized();
+}
+
void Curve2D::_bake() const {
if (!baked_cache_dirty) {
return;
@@ -835,19 +848,19 @@ void Curve2D::_bake() const {
// Collect positions and sample tilts and tangents for each baked points.
bpw[0] = points[0].position;
- bfw[0] = points[0].position.bezier_derivative(points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0).normalized();
+ bfw[0] = _calculate_tangent(points[0].position, points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0);
int pidx = 0;
for (int i = 0; i < points.size() - 1; i++) {
for (const KeyValue<real_t, Vector2> &E : midpoints[i]) {
pidx++;
bpw[pidx] = E.value;
- bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key).normalized();
+ bfw[pidx] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key);
}
pidx++;
bpw[pidx] = points[i + 1].position;
- bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0).normalized();
+ bfw[pidx] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0);
}
// Recalculate the baked distances.
@@ -1031,10 +1044,11 @@ Vector2 Curve2D::get_closest_point(const Vector2 &p_to_point) const {
real_t nearest_dist = -1.0f;
for (int i = 0; i < pc - 1; i++) {
+ const real_t interval = baked_dist_cache[i + 1] - baked_dist_cache[i];
Vector2 origin = r[i];
- Vector2 direction = (r[i + 1] - origin) / bake_interval;
+ Vector2 direction = (r[i + 1] - origin) / interval;
- real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+ real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, interval);
Vector2 proj = origin + direction * d;
real_t dist = proj.distance_squared_to(p_to_point);
@@ -1070,10 +1084,13 @@ real_t Curve2D::get_closest_offset(const Vector2 &p_to_point) const {
real_t offset = 0.0f;
for (int i = 0; i < pc - 1; i++) {
+ offset = baked_dist_cache[i];
+
+ const real_t interval = baked_dist_cache[i + 1] - baked_dist_cache[i];
Vector2 origin = r[i];
- Vector2 direction = (r[i + 1] - origin) / bake_interval;
+ Vector2 direction = (r[i + 1] - origin) / interval;
- real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+ real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, interval);
Vector2 proj = origin + direction * d;
real_t dist = proj.distance_squared_to(p_to_point);
@@ -1082,8 +1099,6 @@ real_t Curve2D::get_closest_offset(const Vector2 &p_to_point) const {
nearest = offset + d;
nearest_dist = dist;
}
-
- offset += bake_interval;
}
return nearest;
@@ -1480,6 +1495,19 @@ void Curve3D::_bake_segment3d_even_length(RBMap<real_t, Vector3> &r_bake, real_t
}
}
+Vector3 Curve3D::_calculate_tangent(const Vector3 &p_begin, const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) {
+ // Handle corner cases.
+ if (Math::is_zero_approx(p_t - 0.0f) && p_control_1.is_equal_approx(p_begin)) {
+ return (p_end - p_begin).normalized();
+ }
+
+ if (Math::is_zero_approx(p_t - 1.0f) && p_control_2.is_equal_approx(p_end)) {
+ return (p_end - p_begin).normalized();
+ }
+
+ return p_begin.bezier_derivative(p_control_1, p_control_2, p_end, p_t).normalized();
+}
+
void Curve3D::_bake() const {
if (!baked_cache_dirty) {
return;
@@ -1539,7 +1567,7 @@ void Curve3D::_bake() const {
// Collect positions and sample tilts and tangents for each baked points.
bpw[0] = points[0].position;
- bfw[0] = points[0].position.bezier_derivative(points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0).normalized();
+ bfw[0] = _calculate_tangent(points[0].position, points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0);
btw[0] = points[0].tilt;
int pidx = 0;
@@ -1547,13 +1575,13 @@ void Curve3D::_bake() const {
for (const KeyValue<real_t, Vector3> &E : midpoints[i]) {
pidx++;
bpw[pidx] = E.value;
- bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key).normalized();
+ bfw[pidx] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key);
btw[pidx] = Math::lerp(points[i].tilt, points[i + 1].tilt, E.key);
}
pidx++;
bpw[pidx] = points[i + 1].position;
- bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0).normalized();
+ bfw[pidx] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0);
btw[pidx] = points[i + 1].tilt;
}
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index ea3ceabb14..26608c47cd 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -178,6 +178,7 @@ class Curve2D : public Resource {
void mark_dirty();
+ static Vector2 _calculate_tangent(const Vector2 &p_begin, const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t);
void _bake() const;
real_t bake_interval = 5.0;
@@ -261,6 +262,7 @@ class Curve3D : public Resource {
void mark_dirty();
+ static Vector3 _calculate_tangent(const Vector3 &p_begin, const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t);
void _bake() const;
struct Interval {
diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index 7b7c67bbc2..79e084e90e 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -161,6 +161,11 @@ void GodotBodyPair2D::_validate_contacts() {
}
}
+// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
+// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
+// Process: only proceed if body A's motion is high relative to its size.
+// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
+// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
Vector2 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
@@ -180,24 +185,32 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
return false;
}
- // Going too fast in that direction.
+ // A is moving fast enough that tunneling might occur. See if it's really about to collide.
// Cast a segment from support in motion normal, in the same direction of motion by motion length.
- // Support is the worst case collision point, so real collision happened before.
+ // Support point will the farthest forward collision point along the movement vector.
+ // i.e. the point that should hit B first if any collision does occur.
+
+ // convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
int a;
Vector2 s[2];
- p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
+ p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
Vector2 from = p_xform_A.xform(s[0]);
+ // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+ // This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
+ from -= motion * 0.1;
Vector2 to = from + motion;
Transform2D from_inv = p_xform_B.affine_inverse();
// Start from a little inside the bounding box.
- Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
+ Vector2 local_from = from_inv.xform(from);
Vector2 local_to = from_inv.xform(to);
Vector2 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
+ // there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
+ // actually collide yet on next frame. We'll probably check again next frame once they're closer.
return false;
}
@@ -215,7 +228,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
// next frame will hit softly or soft enough.
Vector2 hitpos = p_xform_B.xform(rpos);
- real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+ real_t newlen = hitpos.distance_to(from);
p_A->set_linear_velocity(mnormal * (newlen / p_step));
return true;
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 7e6cc6f834..981a7c502f 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -161,6 +161,11 @@ void GodotBodyPair3D::validate_contacts() {
}
}
+// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
+// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
+// Process: only proceed if body A's motion is high relative to its size.
+// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
+// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
Vector3 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
@@ -177,33 +182,39 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
// Let's say it should move more than 1/3 the size of the object in that axis.
bool fast_object = mlen > (max - min) * 0.3;
if (!fast_object) {
- return false;
+ return false; // moving slow enough that there's no chance of tunneling.
}
- // Going too fast in that direction.
+ // A is moving fast enough that tunneling might occur. See if it's really about to collide.
// Cast a segment from support in motion normal, in the same direction of motion by motion length.
- // Support is the worst case collision point, so real collision happened before.
- Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
+ // Support point will the farthest forward collision point along the movement vector.
+ // i.e. the point that should hit B first if any collision does occur.
+
+ // convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
+ Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform_inv(mnormal).normalized());
Vector3 from = p_xform_A.xform(s);
+ // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+ // This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
+ from -= motion * 0.1;
Vector3 to = from + motion;
Transform3D from_inv = p_xform_B.affine_inverse();
- // Start from a little inside the bounding box.
- Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
+ Vector3 local_from = from_inv.xform(from);
Vector3 local_to = from_inv.xform(to);
Vector3 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
+ // there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
+ // actually collide yet on next frame. We'll probably check again next frame once they're closer.
return false;
}
- // Shorten the linear velocity so it does not hit, but gets close enough,
- // next frame will hit softly or soft enough.
+ // Shorten the linear velocity so it will collide next frame.
Vector3 hitpos = p_xform_B.xform(rpos);
- real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+ real_t newlen = hitpos.distance_to(from); // this length (speed) should cause the point we chose slightly behind A's support point to arrive right at B's collider next frame.
p_A->set_linear_velocity((mnormal * newlen) / p_step);
return true;