mirror of
https://github.com/godotengine/godot.git
synced 2024-11-12 23:24:26 +00:00
Refactor Curv3D and PathFollow3D
This commit makes the following major changes 1. Add "sample_baked_with_rotation()" to Curve3D, making it usable independently. A similar change was made to Curve2D previously. 2. Refactor the _bake() method on Curve3D, using Parallel Transport Frame instead of Frenet Frame. 3. Refactor the sample_* methods, including: i. Factor out common binary search code, following the DRY principe ii. sample_up_vector() interpolated up vector as part of rotation frame(posture) for consistancy and accuracy.
This commit is contained in:
Yaohua Xiong
parent
6521eccaae
commit
5241464a46
@ -132,6 +132,15 @@
|
||||
If the curve has no up vectors, the function sends an error to the console, and returns [code](0, 1, 0)[/code].
|
||||
</description>
|
||||
</method>
|
||||
<method name="sample_baked_with_rotation" qualifiers="const">
|
||||
<return type="Transform3D" />
|
||||
<param index="0" name="offset" type="float" />
|
||||
<param index="1" name="cubic" type="bool" default="false" />
|
||||
<param index="2" name="apply_tilt" type="bool" default="false" />
|
||||
<description>
|
||||
Similar with [code]interpolate_baked()[/code]. The the return value is [code]Transform3D[/code], with [code]origin[/code] as point position, [code]basis.x[/code] as sideway vector, [code]basis.y[/code] as up vector, [code]basis.z[/code] as forward vector. When the curve length is 0, there is no reasonable way to caculate the rotation, all vectors aligned with global space axes.
|
||||
</description>
|
||||
</method>
|
||||
<method name="samplef" qualifiers="const">
|
||||
<return type="Vector3" />
|
||||
<param index="0" name="fofs" type="float" />
|
||||
|
||||
@ -9,6 +9,16 @@
|
||||
</description>
|
||||
<tutorials>
|
||||
</tutorials>
|
||||
<methods>
|
||||
<method name="correct_posture" qualifiers="static">
|
||||
<return type="Transform3D" />
|
||||
<param index="0" name="transform" type="Transform3D" />
|
||||
<param index="1" name="rotation_mode" type="int" enum="PathFollow3D.RotationMode" />
|
||||
<description>
|
||||
Correct the [code]transform[/code]. [code]rotation_mode[/code] implicitly specifies how posture (forward, up and sideway direction) is caculated.
|
||||
</description>
|
||||
</method>
|
||||
</methods>
|
||||
<members>
|
||||
<member name="cubic_interp" type="bool" setter="set_cubic_interpolation" getter="get_cubic_interpolation" default="true">
|
||||
If [code]true[/code], the position between two cached points is interpolated cubically, and linearly otherwise.
|
||||
@ -30,6 +40,9 @@
|
||||
<member name="rotation_mode" type="int" setter="set_rotation_mode" getter="get_rotation_mode" enum="PathFollow3D.RotationMode" default="3">
|
||||
Allows or forbids rotation on one or more axes, depending on the [enum RotationMode] constants being used.
|
||||
</member>
|
||||
<member name="tilt_enabled" type="bool" setter="set_tilt_enabled" getter="is_tilt_enabled" default="true">
|
||||
If [code]true[/code], the tilt property of [Curve3D] takes effect.
|
||||
</member>
|
||||
<member name="v_offset" type="float" setter="set_v_offset" getter="get_v_offset" default="0.0">
|
||||
The node's offset perpendicular to the curve.
|
||||
</member>
|
||||
|
||||
@ -182,125 +182,31 @@ void PathFollow3D::_update_transform(bool p_update_xyz_rot) {
|
||||
if (bl == 0.0) {
|
||||
return;
|
||||
}
|
||||
real_t bi = c->get_bake_interval();
|
||||
real_t o_next = progress + bi;
|
||||
real_t o_prev = progress - bi;
|
||||
|
||||
if (loop) {
|
||||
o_next = Math::fposmod(o_next, bl);
|
||||
o_prev = Math::fposmod(o_prev, bl);
|
||||
} else if (rotation_mode == ROTATION_ORIENTED) {
|
||||
if (o_next >= bl) {
|
||||
o_next = bl;
|
||||
}
|
||||
if (o_prev <= 0) {
|
||||
o_prev = 0;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 pos = c->sample_baked(progress, cubic);
|
||||
Transform3D t = get_transform();
|
||||
// Vector3 pos_offset = Vector3(h_offset, v_offset, 0); not used in all cases
|
||||
// will be replaced by "Vector3(h_offset, v_offset, 0)" where it was formerly used
|
||||
|
||||
if (rotation_mode == ROTATION_ORIENTED) {
|
||||
Vector3 forward = c->sample_baked(o_next, cubic) - pos;
|
||||
|
||||
// Try with the previous position
|
||||
if (forward.length_squared() < CMP_EPSILON2) {
|
||||
forward = pos - c->sample_baked(o_prev, cubic);
|
||||
}
|
||||
|
||||
if (forward.length_squared() < CMP_EPSILON2) {
|
||||
forward = Vector3(0, 0, 1);
|
||||
} else {
|
||||
forward.normalize();
|
||||
}
|
||||
|
||||
Vector3 up = c->sample_baked_up_vector(progress, true);
|
||||
|
||||
if (o_next < progress) {
|
||||
Vector3 up1 = c->sample_baked_up_vector(o_next, true);
|
||||
Vector3 axis = up.cross(up1);
|
||||
|
||||
if (axis.length_squared() < CMP_EPSILON2) {
|
||||
axis = forward;
|
||||
} else {
|
||||
axis.normalize();
|
||||
}
|
||||
|
||||
up.rotate(axis, up.angle_to(up1) * 0.5f);
|
||||
}
|
||||
|
||||
Vector3 scale = t.basis.get_scale();
|
||||
Vector3 sideways = up.cross(forward).normalized();
|
||||
up = forward.cross(sideways).normalized();
|
||||
|
||||
t.basis.set_columns(sideways, up, forward);
|
||||
t.basis.scale_local(scale);
|
||||
|
||||
t.origin = pos + sideways * h_offset + up * v_offset;
|
||||
} else if (rotation_mode != ROTATION_NONE) {
|
||||
// perform parallel transport
|
||||
//
|
||||
// see C. Dougan, The Parallel Transport Frame, Game Programming Gems 2 for example
|
||||
// for a discussion about why not Frenet frame.
|
||||
Transform3D t;
|
||||
|
||||
if (rotation_mode == ROTATION_NONE) {
|
||||
Vector3 pos = c->sample_baked(progress, cubic);
|
||||
t.origin = pos;
|
||||
if (p_update_xyz_rot && prev_offset != progress) { // Only update rotation if some parameter has changed - i.e. not on addition to scene tree.
|
||||
real_t sample_distance = bi * 0.01;
|
||||
Vector3 t_prev_pos_a = c->sample_baked(prev_offset - sample_distance, cubic);
|
||||
Vector3 t_prev_pos_b = c->sample_baked(prev_offset + sample_distance, cubic);
|
||||
Vector3 t_cur_pos_a = c->sample_baked(progress - sample_distance, cubic);
|
||||
Vector3 t_cur_pos_b = c->sample_baked(progress + sample_distance, cubic);
|
||||
Vector3 t_prev = (t_prev_pos_a - t_prev_pos_b).normalized();
|
||||
Vector3 t_cur = (t_cur_pos_a - t_cur_pos_b).normalized();
|
||||
|
||||
Vector3 axis = t_prev.cross(t_cur);
|
||||
real_t dot = t_prev.dot(t_cur);
|
||||
real_t angle = Math::acos(CLAMP(dot, -1, 1));
|
||||
|
||||
if (likely(!Math::is_zero_approx(angle))) {
|
||||
if (rotation_mode == ROTATION_Y) {
|
||||
// assuming we're referring to global Y-axis. is this correct?
|
||||
axis.x = 0;
|
||||
axis.z = 0;
|
||||
} else if (rotation_mode == ROTATION_XY) {
|
||||
axis.z = 0;
|
||||
} else if (rotation_mode == ROTATION_XYZ) {
|
||||
// all components are allowed
|
||||
}
|
||||
|
||||
if (likely(!Math::is_zero_approx(axis.length()))) {
|
||||
t.rotate_basis(axis.normalized(), angle);
|
||||
}
|
||||
}
|
||||
|
||||
// do the additional tilting
|
||||
real_t tilt_angle = c->sample_baked_tilt(progress);
|
||||
Vector3 tilt_axis = t_cur; // not sure what tilt is supposed to do, is this correct??
|
||||
|
||||
if (likely(!Math::is_zero_approx(Math::abs(tilt_angle)))) {
|
||||
if (rotation_mode == ROTATION_Y) {
|
||||
tilt_axis.x = 0;
|
||||
tilt_axis.z = 0;
|
||||
} else if (rotation_mode == ROTATION_XY) {
|
||||
tilt_axis.z = 0;
|
||||
} else if (rotation_mode == ROTATION_XYZ) {
|
||||
// all components are allowed
|
||||
}
|
||||
|
||||
if (likely(!Math::is_zero_approx(tilt_axis.length()))) {
|
||||
t.rotate_basis(tilt_axis.normalized(), tilt_angle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
t.translate_local(Vector3(h_offset, v_offset, 0));
|
||||
} else {
|
||||
t.origin = pos + Vector3(h_offset, v_offset, 0);
|
||||
t = c->sample_baked_with_rotation(progress, cubic, false);
|
||||
Vector3 forward = t.basis.get_column(2); // Retain tangent for applying tilt
|
||||
t = PathFollow3D::correct_posture(t, rotation_mode);
|
||||
|
||||
// Apply tilt *after* correct_posture
|
||||
if (tilt_enabled) {
|
||||
const real_t tilt = c->sample_baked_tilt(progress);
|
||||
|
||||
const Basis twist(forward, tilt);
|
||||
t.basis = twist * t.basis;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 scale = get_transform().basis.get_scale();
|
||||
|
||||
t.translate_local(Vector3(h_offset, v_offset, 0));
|
||||
t.basis.scale_local(scale);
|
||||
|
||||
set_transform(t);
|
||||
}
|
||||
|
||||
@ -358,6 +264,38 @@ PackedStringArray PathFollow3D::get_configuration_warnings() const {
|
||||
return warnings;
|
||||
}
|
||||
|
||||
Transform3D PathFollow3D::correct_posture(Transform3D p_transform, PathFollow3D::RotationMode p_rotation_mode) {
|
||||
Transform3D t = p_transform;
|
||||
|
||||
// Modify frame according to rotation mode.
|
||||
if (p_rotation_mode == PathFollow3D::ROTATION_NONE) {
|
||||
// Clear rotation.
|
||||
t.basis = Basis();
|
||||
} else if (p_rotation_mode == PathFollow3D::ROTATION_ORIENTED) {
|
||||
// Y-axis always straight up.
|
||||
Vector3 up(0.0, 1.0, 0.0);
|
||||
Vector3 forward = t.basis.get_column(2);
|
||||
|
||||
t.basis = Basis::looking_at(-forward, up);
|
||||
} else {
|
||||
// Lock some euler axes.
|
||||
Vector3 euler = t.basis.get_euler_normalized(EulerOrder::YXZ);
|
||||
if (p_rotation_mode == PathFollow3D::ROTATION_Y) {
|
||||
// Only Y-axis allowed.
|
||||
euler[0] = 0;
|
||||
euler[2] = 0;
|
||||
} else if (p_rotation_mode == PathFollow3D::ROTATION_XY) {
|
||||
// XY allowed.
|
||||
euler[2] = 0;
|
||||
}
|
||||
|
||||
Basis locked = Basis::from_euler(euler, EulerOrder::YXZ);
|
||||
t.basis = locked;
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
void PathFollow3D::_bind_methods() {
|
||||
ClassDB::bind_method(D_METHOD("set_progress", "progress"), &PathFollow3D::set_progress);
|
||||
ClassDB::bind_method(D_METHOD("get_progress"), &PathFollow3D::get_progress);
|
||||
@ -380,6 +318,11 @@ void PathFollow3D::_bind_methods() {
|
||||
ClassDB::bind_method(D_METHOD("set_loop", "loop"), &PathFollow3D::set_loop);
|
||||
ClassDB::bind_method(D_METHOD("has_loop"), &PathFollow3D::has_loop);
|
||||
|
||||
ClassDB::bind_method(D_METHOD("set_tilt_enabled", "enabled"), &PathFollow3D::set_tilt_enabled);
|
||||
ClassDB::bind_method(D_METHOD("is_tilt_enabled"), &PathFollow3D::is_tilt_enabled);
|
||||
|
||||
ClassDB::bind_static_method("PathFollow3D", D_METHOD("correct_posture", "transform", "rotation_mode"), &PathFollow3D::correct_posture);
|
||||
|
||||
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "progress", PROPERTY_HINT_RANGE, "0,10000,0.01,or_less,or_greater,suffix:m"), "set_progress", "get_progress");
|
||||
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "progress_ratio", PROPERTY_HINT_RANGE, "0,1,0.0001,or_less,or_greater", PROPERTY_USAGE_EDITOR), "set_progress_ratio", "get_progress_ratio");
|
||||
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "h_offset", PROPERTY_HINT_NONE, "suffix:m"), "set_h_offset", "get_h_offset");
|
||||
@ -387,6 +330,7 @@ void PathFollow3D::_bind_methods() {
|
||||
ADD_PROPERTY(PropertyInfo(Variant::INT, "rotation_mode", PROPERTY_HINT_ENUM, "None,Y,XY,XYZ,Oriented"), "set_rotation_mode", "get_rotation_mode");
|
||||
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "cubic_interp"), "set_cubic_interpolation", "get_cubic_interpolation");
|
||||
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "loop"), "set_loop", "has_loop");
|
||||
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "tilt_enabled"), "set_tilt_enabled", "is_tilt_enabled");
|
||||
|
||||
BIND_ENUM_CONSTANT(ROTATION_NONE);
|
||||
BIND_ENUM_CONSTANT(ROTATION_Y);
|
||||
@ -397,7 +341,6 @@ void PathFollow3D::_bind_methods() {
|
||||
|
||||
void PathFollow3D::set_progress(real_t p_progress) {
|
||||
ERR_FAIL_COND(!isfinite(p_progress));
|
||||
prev_offset = progress;
|
||||
progress = p_progress;
|
||||
|
||||
if (path) {
|
||||
@ -409,8 +352,6 @@ void PathFollow3D::set_progress(real_t p_progress) {
|
||||
if (!Math::is_zero_approx(p_progress) && Math::is_zero_approx(progress)) {
|
||||
progress = path_length;
|
||||
}
|
||||
} else {
|
||||
progress = CLAMP(progress, 0, path_length);
|
||||
}
|
||||
}
|
||||
|
||||
@ -476,3 +417,11 @@ void PathFollow3D::set_loop(bool p_loop) {
|
||||
bool PathFollow3D::has_loop() const {
|
||||
return loop;
|
||||
}
|
||||
|
||||
void PathFollow3D::set_tilt_enabled(bool p_enable) {
|
||||
tilt_enabled = p_enable;
|
||||
}
|
||||
|
||||
bool PathFollow3D::is_tilt_enabled() const {
|
||||
return tilt_enabled;
|
||||
}
|
||||
|
||||
@ -72,14 +72,16 @@ public:
|
||||
ROTATION_ORIENTED
|
||||
};
|
||||
|
||||
static Transform3D correct_posture(Transform3D p_transform, PathFollow3D::RotationMode p_rotation_mode);
|
||||
|
||||
private:
|
||||
Path3D *path = nullptr;
|
||||
real_t prev_offset = 0.0; // Offset during the last _update_transform.
|
||||
real_t progress = 0.0;
|
||||
real_t h_offset = 0.0;
|
||||
real_t v_offset = 0.0;
|
||||
bool cubic = true;
|
||||
bool loop = true;
|
||||
bool tilt_enabled = true;
|
||||
RotationMode rotation_mode = ROTATION_XYZ;
|
||||
|
||||
void _update_transform(bool p_update_xyz_rot = true);
|
||||
@ -106,6 +108,9 @@ public:
|
||||
void set_loop(bool p_loop);
|
||||
bool has_loop() const;
|
||||
|
||||
void set_tilt_enabled(bool p_enable);
|
||||
bool is_tilt_enabled() const;
|
||||
|
||||
void set_rotation_mode(RotationMode p_rotation_mode);
|
||||
RotationMode get_rotation_mode() const;
|
||||
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
#include "curve.h"
|
||||
|
||||
#include "core/core_string_names.h"
|
||||
#include "core/math/math_funcs.h"
|
||||
|
||||
const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed";
|
||||
|
||||
@ -1413,8 +1414,9 @@ void Curve3D::_bake() const {
|
||||
if (points.size() == 0) {
|
||||
baked_point_cache.clear();
|
||||
baked_tilt_cache.clear();
|
||||
baked_up_vector_cache.clear();
|
||||
baked_dist_cache.clear();
|
||||
|
||||
baked_up_vector_cache.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -1438,15 +1440,16 @@ void Curve3D::_bake() const {
|
||||
|
||||
Vector3 position = points[0].position;
|
||||
real_t dist = 0.0;
|
||||
List<Plane> pointlist;
|
||||
List<Plane> pointlist; // Abuse Plane for (position, dist)
|
||||
List<real_t> distlist;
|
||||
|
||||
// Start always from origin.
|
||||
pointlist.push_back(Plane(position, points[0].tilt));
|
||||
distlist.push_back(0.0);
|
||||
|
||||
// Step 1: Sample points
|
||||
const real_t step = 0.1; // At least 10 substeps ought to be enough?
|
||||
for (int i = 0; i < points.size() - 1; i++) {
|
||||
real_t step = 0.1; // at least 10 substeps ought to be enough?
|
||||
real_t p = 0.0;
|
||||
|
||||
while (p < 1.0) {
|
||||
@ -1461,7 +1464,7 @@ void Curve3D::_bake() const {
|
||||
if (d > bake_interval) {
|
||||
// OK! between P and NP there _has_ to be Something, let's go searching!
|
||||
|
||||
int iterations = 10; //lots of detail!
|
||||
const int iterations = 10; // Lots of detail!
|
||||
|
||||
real_t low = p;
|
||||
real_t hi = np;
|
||||
@ -1496,76 +1499,135 @@ void Curve3D::_bake() const {
|
||||
Vector3 npp = points[i + 1].position;
|
||||
real_t d = position.distance_to(npp);
|
||||
|
||||
position = npp;
|
||||
Plane post;
|
||||
post.normal = position;
|
||||
post.d = points[i + 1].tilt;
|
||||
if (d > CMP_EPSILON) { // Avoid the degenerate case of two very close points.
|
||||
position = npp;
|
||||
Plane post;
|
||||
post.normal = position;
|
||||
post.d = points[i + 1].tilt;
|
||||
|
||||
dist += d;
|
||||
dist += d;
|
||||
|
||||
pointlist.push_back(post);
|
||||
distlist.push_back(dist);
|
||||
pointlist.push_back(post);
|
||||
distlist.push_back(dist);
|
||||
}
|
||||
}
|
||||
|
||||
baked_max_ofs = dist;
|
||||
|
||||
baked_point_cache.resize(pointlist.size());
|
||||
Vector3 *w = baked_point_cache.ptrw();
|
||||
int idx = 0;
|
||||
const int point_count = pointlist.size();
|
||||
{
|
||||
baked_point_cache.resize(point_count);
|
||||
Vector3 *w = baked_point_cache.ptrw();
|
||||
|
||||
baked_tilt_cache.resize(pointlist.size());
|
||||
real_t *wt = baked_tilt_cache.ptrw();
|
||||
baked_tilt_cache.resize(point_count);
|
||||
real_t *wt = baked_tilt_cache.ptrw();
|
||||
|
||||
baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0);
|
||||
Vector3 *up_write = baked_up_vector_cache.ptrw();
|
||||
baked_dist_cache.resize(point_count);
|
||||
real_t *wd = baked_dist_cache.ptrw();
|
||||
|
||||
baked_dist_cache.resize(pointlist.size());
|
||||
real_t *wd = baked_dist_cache.ptrw();
|
||||
int idx = 0;
|
||||
for (const Plane &E : pointlist) {
|
||||
w[idx] = E.normal;
|
||||
wt[idx] = E.d;
|
||||
wd[idx] = distlist[idx];
|
||||
|
||||
Vector3 sideways;
|
||||
Vector3 up;
|
||||
Vector3 forward;
|
||||
|
||||
Vector3 prev_sideways = Vector3(1, 0, 0);
|
||||
Vector3 prev_up = Vector3(0, 1, 0);
|
||||
Vector3 prev_forward = Vector3(0, 0, 1);
|
||||
|
||||
for (const Plane &E : pointlist) {
|
||||
w[idx] = E.normal;
|
||||
wt[idx] = E.d;
|
||||
wd[idx] = distlist[idx];
|
||||
|
||||
if (!up_vector_enabled) {
|
||||
idx++;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (!up_vector_enabled) {
|
||||
baked_up_vector_cache.resize(0);
|
||||
return;
|
||||
}
|
||||
|
||||
// Step 2: Calculate the up vectors and the whole local reference frame
|
||||
//
|
||||
// See Dougan, Carl. "The parallel transport frame." Game Programming Gems 2 (2001): 215-219.
|
||||
// for an example discussing about why not the Frenet frame.
|
||||
{
|
||||
PackedVector3Array forward_vectors;
|
||||
|
||||
baked_up_vector_cache.resize(point_count);
|
||||
forward_vectors.resize(point_count);
|
||||
|
||||
Vector3 *up_write = baked_up_vector_cache.ptrw();
|
||||
Vector3 *forward_write = forward_vectors.ptrw();
|
||||
|
||||
const Vector3 *points_ptr = baked_point_cache.ptr();
|
||||
|
||||
Basis frame; // X-right, Y-up, Z-forward.
|
||||
Basis frame_prev;
|
||||
|
||||
// Set the initial frame based on Y-up rule.
|
||||
{
|
||||
Vector3 up(0, 1, 0);
|
||||
Vector3 forward = (points_ptr[1] - points_ptr[0]).normalized();
|
||||
if (forward.is_equal_approx(Vector3())) {
|
||||
forward = Vector3(1, 0, 0);
|
||||
}
|
||||
|
||||
if (abs(forward.dot(up)) > 1.0 - UNIT_EPSILON) {
|
||||
frame_prev = Basis::looking_at(-forward, up);
|
||||
} else {
|
||||
frame_prev = Basis::looking_at(-forward, Vector3(1, 0, 0));
|
||||
}
|
||||
|
||||
up_write[0] = frame_prev.get_column(1);
|
||||
forward_write[0] = frame_prev.get_column(2);
|
||||
}
|
||||
|
||||
forward = idx > 0 ? (w[idx] - w[idx - 1]).normalized() : prev_forward;
|
||||
// Calculate the Parallel Transport Frame.
|
||||
for (int idx = 1; idx < point_count; idx++) {
|
||||
Vector3 forward = (points_ptr[idx] - points_ptr[idx - 1]).normalized();
|
||||
if (forward.is_equal_approx(Vector3())) {
|
||||
forward = frame_prev.get_column(2);
|
||||
}
|
||||
|
||||
real_t y_dot = prev_up.dot(forward);
|
||||
Basis rotate;
|
||||
rotate.rotate_to_align(frame_prev.get_column(2), forward);
|
||||
frame = rotate * frame_prev;
|
||||
frame.orthonormalize(); // guard against float error accumulation
|
||||
|
||||
if (y_dot > (1.0f - CMP_EPSILON)) {
|
||||
sideways = prev_sideways;
|
||||
up = -prev_forward;
|
||||
} else if (y_dot < -(1.0f - CMP_EPSILON)) {
|
||||
sideways = prev_sideways;
|
||||
up = prev_forward;
|
||||
} else {
|
||||
sideways = prev_up.cross(forward).normalized();
|
||||
up = forward.cross(sideways).normalized();
|
||||
up_write[idx] = frame.get_column(1);
|
||||
forward_write[idx] = frame.get_column(2);
|
||||
|
||||
frame_prev = frame;
|
||||
}
|
||||
|
||||
if (idx == 1) {
|
||||
up_write[0] = up;
|
||||
bool is_loop = true;
|
||||
// Loop smoothing only applies when the curve is a loop, which means two ends meet, and share forward directions.
|
||||
{
|
||||
if (!points_ptr[0].is_equal_approx(points_ptr[point_count - 1])) {
|
||||
is_loop = false;
|
||||
}
|
||||
|
||||
real_t dot = forward_write[0].dot(forward_write[point_count - 1]);
|
||||
if (dot < 1.0 - 0.01) { // Alignment should not be too tight, or it dosen't work for coarse bake interval
|
||||
is_loop = false;
|
||||
}
|
||||
}
|
||||
|
||||
up_write[idx] = up;
|
||||
// Twist up vectors, so that they align at two ends of the curve.
|
||||
if (is_loop) {
|
||||
const Vector3 up_start = up_write[0];
|
||||
const Vector3 up_end = up_write[point_count - 1];
|
||||
|
||||
prev_sideways = sideways;
|
||||
prev_up = up;
|
||||
prev_forward = forward;
|
||||
real_t sign = SIGN(up_end.cross(up_start).dot(forward_write[0]));
|
||||
real_t full_angle = Quaternion(up_end, up_start).get_angle();
|
||||
|
||||
idx++;
|
||||
if (abs(full_angle) < UNIT_EPSILON) {
|
||||
return;
|
||||
} else {
|
||||
const real_t *dists = baked_dist_cache.ptr();
|
||||
for (int idx = 1; idx < point_count; idx++) {
|
||||
const real_t frac = dists[idx] / baked_max_ofs;
|
||||
const real_t angle = Math::lerp((real_t)0.0, full_angle, frac);
|
||||
Basis twist(forward_write[idx] * sign, angle);
|
||||
|
||||
up_write[idx] = twist.xform(up_write[idx]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1577,27 +1639,15 @@ real_t Curve3D::get_baked_length() const {
|
||||
return baked_max_ofs;
|
||||
}
|
||||
|
||||
Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
|
||||
if (baked_cache_dirty) {
|
||||
_bake();
|
||||
}
|
||||
Curve3D::Interval Curve3D::_find_interval(real_t p_offset) const {
|
||||
Interval interval = {
|
||||
-1,
|
||||
0.0
|
||||
};
|
||||
ERR_FAIL_COND_V_MSG(baked_cache_dirty, interval, "Backed cache is dirty");
|
||||
|
||||
// Validate: Curve may not have baked points.
|
||||
int pc = baked_point_cache.size();
|
||||
ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D.");
|
||||
|
||||
if (pc == 1) {
|
||||
return baked_point_cache.get(0);
|
||||
}
|
||||
|
||||
const Vector3 *r = baked_point_cache.ptr();
|
||||
|
||||
if (p_offset < 0) {
|
||||
return r[0];
|
||||
}
|
||||
if (p_offset >= baked_max_ofs) {
|
||||
return r[pc - 1];
|
||||
}
|
||||
ERR_FAIL_COND_V_MSG(pc < 2, interval, "Less than two points in cache");
|
||||
|
||||
int start = 0;
|
||||
int end = pc;
|
||||
@ -1617,9 +1667,27 @@ Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
|
||||
real_t offset_end = baked_dist_cache[idx + 1];
|
||||
|
||||
real_t idx_interval = offset_end - offset_begin;
|
||||
ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(), "Couldn't find baked segment.");
|
||||
ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, interval, "Offset out of range.");
|
||||
|
||||
real_t frac = (p_offset - offset_begin) / idx_interval;
|
||||
interval.idx = idx;
|
||||
if (idx_interval < FLT_EPSILON) {
|
||||
interval.frac = 0.5; // For a very short interval, 0.5 is a reasonable choice.
|
||||
ERR_FAIL_V_MSG(interval, "Zero length interval.");
|
||||
}
|
||||
|
||||
interval.frac = (p_offset - offset_begin) / idx_interval;
|
||||
return interval;
|
||||
}
|
||||
|
||||
Vector3 Curve3D::_sample_baked(Interval p_interval, bool p_cubic) const {
|
||||
// Assuming p_interval is valid.
|
||||
ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Vector3(), "Invalid interval");
|
||||
|
||||
int idx = p_interval.idx;
|
||||
real_t frac = p_interval.frac;
|
||||
|
||||
const Vector3 *r = baked_point_cache.ptr();
|
||||
int pc = baked_point_cache.size();
|
||||
|
||||
if (p_cubic) {
|
||||
Vector3 pre = idx > 0 ? r[idx - 1] : r[idx];
|
||||
@ -1630,6 +1698,114 @@ Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
|
||||
}
|
||||
}
|
||||
|
||||
real_t Curve3D::_sample_baked_tilt(Interval p_interval) const {
|
||||
// Assuming that p_interval is valid.
|
||||
ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_tilt_cache.size(), 0.0, "Invalid interval");
|
||||
|
||||
int idx = p_interval.idx;
|
||||
real_t frac = p_interval.frac;
|
||||
|
||||
const real_t *r = baked_tilt_cache.ptr();
|
||||
|
||||
return Math::lerp(r[idx], r[idx + 1], frac);
|
||||
}
|
||||
|
||||
Basis Curve3D::_sample_posture(Interval p_interval, bool p_apply_tilt) const {
|
||||
// Assuming that p_interval is valid.
|
||||
ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Basis(), "Invalid interval");
|
||||
if (up_vector_enabled) {
|
||||
ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_up_vector_cache.size(), Basis(), "Invalid interval");
|
||||
}
|
||||
|
||||
int idx = p_interval.idx;
|
||||
real_t frac = p_interval.frac;
|
||||
|
||||
Vector3 forward_begin;
|
||||
Vector3 forward_end;
|
||||
if (idx == 0) {
|
||||
forward_begin = (baked_point_cache[1] - baked_point_cache[0]).normalized();
|
||||
forward_end = (baked_point_cache[1] - baked_point_cache[0]).normalized();
|
||||
} else {
|
||||
forward_begin = (baked_point_cache[idx] - baked_point_cache[idx - 1]).normalized();
|
||||
forward_end = (baked_point_cache[idx + 1] - baked_point_cache[idx]).normalized();
|
||||
}
|
||||
|
||||
Vector3 up_begin;
|
||||
Vector3 up_end;
|
||||
if (up_vector_enabled) {
|
||||
const Vector3 *up_ptr = baked_up_vector_cache.ptr();
|
||||
up_begin = up_ptr[idx];
|
||||
up_end = up_ptr[idx + 1];
|
||||
} else {
|
||||
up_begin = Vector3(0.0, 1.0, 0.0);
|
||||
up_end = Vector3(0.0, 1.0, 0.0);
|
||||
}
|
||||
|
||||
// Build frames at both ends of the interval, then interpolate.
|
||||
const Basis frame_begin = Basis::looking_at(-forward_begin, up_begin);
|
||||
const Basis frame_end = Basis::looking_at(-forward_end, up_end);
|
||||
const Basis frame = frame_begin.slerp(frame_end, frac).orthonormalized();
|
||||
|
||||
if (!p_apply_tilt) {
|
||||
return frame;
|
||||
}
|
||||
|
||||
// Applying tilt.
|
||||
const real_t tilt = _sample_baked_tilt(p_interval);
|
||||
Vector3 forward = frame.get_column(2);
|
||||
|
||||
const Basis twist(forward, tilt);
|
||||
return twist * frame;
|
||||
}
|
||||
|
||||
Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
|
||||
if (baked_cache_dirty) {
|
||||
_bake();
|
||||
}
|
||||
|
||||
// Validate: Curve may not have baked points.
|
||||
int pc = baked_point_cache.size();
|
||||
ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D.");
|
||||
|
||||
if (pc == 1) {
|
||||
return baked_point_cache[0];
|
||||
}
|
||||
|
||||
p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
|
||||
|
||||
Curve3D::Interval interval = _find_interval(p_offset);
|
||||
return _sample_baked(interval, p_cubic);
|
||||
}
|
||||
|
||||
Transform3D Curve3D::sample_baked_with_rotation(real_t p_offset, bool p_cubic, bool p_apply_tilt) const {
|
||||
if (baked_cache_dirty) {
|
||||
_bake();
|
||||
}
|
||||
|
||||
// Validate: Curve may not have baked points.
|
||||
const int point_count = baked_point_cache.size();
|
||||
ERR_FAIL_COND_V_MSG(point_count == 0, Transform3D(), "No points in Curve3D.");
|
||||
|
||||
if (point_count == 1) {
|
||||
Transform3D t;
|
||||
t.origin = baked_point_cache.get(0);
|
||||
ERR_FAIL_V_MSG(t, "Only 1 point in Curve3D.");
|
||||
}
|
||||
|
||||
p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
|
||||
|
||||
// 0. Find interval for all sampling steps.
|
||||
Curve3D::Interval interval = _find_interval(p_offset);
|
||||
|
||||
// 1. Sample position.
|
||||
Vector3 pos = _sample_baked(interval, p_cubic);
|
||||
|
||||
// 2. Sample rotation frame.
|
||||
Basis frame = _sample_posture(interval, p_apply_tilt);
|
||||
|
||||
return Transform3D(frame, pos);
|
||||
}
|
||||
|
||||
real_t Curve3D::sample_baked_tilt(real_t p_offset) const {
|
||||
if (baked_cache_dirty) {
|
||||
_bake();
|
||||
@ -1643,38 +1819,10 @@ real_t Curve3D::sample_baked_tilt(real_t p_offset) const {
|
||||
return baked_tilt_cache.get(0);
|
||||
}
|
||||
|
||||
const real_t *r = baked_tilt_cache.ptr();
|
||||
p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic
|
||||
|
||||
if (p_offset < 0) {
|
||||
return r[0];
|
||||
}
|
||||
if (p_offset >= baked_max_ofs) {
|
||||
return r[pc - 1];
|
||||
}
|
||||
|
||||
int start = 0;
|
||||
int end = pc;
|
||||
int idx = (end + start) / 2;
|
||||
// Binary search to find baked points.
|
||||
while (start < idx) {
|
||||
real_t offset = baked_dist_cache[idx];
|
||||
if (p_offset <= offset) {
|
||||
end = idx;
|
||||
} else {
|
||||
start = idx;
|
||||
}
|
||||
idx = (end + start) / 2;
|
||||
}
|
||||
|
||||
real_t offset_begin = baked_dist_cache[idx];
|
||||
real_t offset_end = baked_dist_cache[idx + 1];
|
||||
|
||||
real_t idx_interval = offset_end - offset_begin;
|
||||
ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, 0, "Couldn't find baked segment.");
|
||||
|
||||
real_t frac = (p_offset - offset_begin) / idx_interval;
|
||||
|
||||
return Math::lerp(r[idx], r[idx + 1], (real_t)frac);
|
||||
Curve3D::Interval interval = _find_interval(p_offset);
|
||||
return _sample_baked_tilt(interval);
|
||||
}
|
||||
|
||||
Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const {
|
||||
@ -1683,61 +1831,17 @@ Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) cons
|
||||
}
|
||||
|
||||
// Validate: Curve may not have baked up vectors.
|
||||
int count = baked_up_vector_cache.size();
|
||||
ERR_FAIL_COND_V_MSG(count == 0, Vector3(0, 1, 0), "No up vectors in Curve3D.");
|
||||
ERR_FAIL_COND_V_MSG(!up_vector_enabled, Vector3(0, 1, 0), "No up vectors in Curve3D.");
|
||||
|
||||
int count = baked_up_vector_cache.size();
|
||||
if (count == 1) {
|
||||
return baked_up_vector_cache.get(0);
|
||||
}
|
||||
|
||||
const Vector3 *r = baked_up_vector_cache.ptr();
|
||||
const Vector3 *rp = baked_point_cache.ptr();
|
||||
const real_t *rt = baked_tilt_cache.ptr();
|
||||
p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
|
||||
|
||||
int start = 0;
|
||||
int end = count;
|
||||
int idx = (end + start) / 2;
|
||||
// Binary search to find baked points.
|
||||
while (start < idx) {
|
||||
real_t offset = baked_dist_cache[idx];
|
||||
if (p_offset <= offset) {
|
||||
end = idx;
|
||||
} else {
|
||||
start = idx;
|
||||
}
|
||||
idx = (end + start) / 2;
|
||||
}
|
||||
|
||||
if (idx == count - 1) {
|
||||
return p_apply_tilt ? r[idx].rotated((rp[idx] - rp[idx - 1]).normalized(), rt[idx]) : r[idx];
|
||||
}
|
||||
|
||||
real_t offset_begin = baked_dist_cache[idx];
|
||||
real_t offset_end = baked_dist_cache[idx + 1];
|
||||
|
||||
real_t idx_interval = offset_end - offset_begin;
|
||||
ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(0, 1, 0), "Couldn't find baked segment.");
|
||||
|
||||
real_t frac = (p_offset - offset_begin) / idx_interval;
|
||||
|
||||
Vector3 forward = (rp[idx + 1] - rp[idx]).normalized();
|
||||
Vector3 up = r[idx];
|
||||
Vector3 up1 = r[idx + 1];
|
||||
|
||||
if (p_apply_tilt) {
|
||||
up.rotate(forward, rt[idx]);
|
||||
up1.rotate(idx + 2 >= count ? forward : (rp[idx + 2] - rp[idx + 1]).normalized(), rt[idx + 1]);
|
||||
}
|
||||
|
||||
Vector3 axis = up.cross(up1);
|
||||
|
||||
if (axis.length_squared() < CMP_EPSILON2) {
|
||||
axis = forward;
|
||||
} else {
|
||||
axis.normalize();
|
||||
}
|
||||
|
||||
return up.rotated(axis, up.angle_to(up1) * frac);
|
||||
Curve3D::Interval interval = _find_interval(p_offset);
|
||||
return _sample_posture(interval, p_apply_tilt).get_column(1);
|
||||
}
|
||||
|
||||
PackedVector3Array Curve3D::get_baked_points() const {
|
||||
@ -2034,6 +2138,7 @@ void Curve3D::_bind_methods() {
|
||||
|
||||
ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve3D::get_baked_length);
|
||||
ClassDB::bind_method(D_METHOD("sample_baked", "offset", "cubic"), &Curve3D::sample_baked, DEFVAL(false));
|
||||
ClassDB::bind_method(D_METHOD("sample_baked_with_rotation", "offset", "cubic", "apply_tilt"), &Curve3D::sample_baked_with_rotation, DEFVAL(false), DEFVAL(false));
|
||||
ClassDB::bind_method(D_METHOD("sample_baked_up_vector", "offset", "apply_tilt"), &Curve3D::sample_baked_up_vector, DEFVAL(false));
|
||||
ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve3D::get_baked_points);
|
||||
ClassDB::bind_method(D_METHOD("get_baked_tilts"), &Curve3D::get_baked_tilts);
|
||||
|
||||
@ -238,11 +238,6 @@ class Curve3D : public Resource {
|
||||
|
||||
Vector<Point> points;
|
||||
|
||||
struct BakedPoint {
|
||||
real_t ofs = 0.0;
|
||||
Vector3 point;
|
||||
};
|
||||
|
||||
mutable bool baked_cache_dirty = false;
|
||||
mutable PackedVector3Array baked_point_cache;
|
||||
mutable Vector<real_t> baked_tilt_cache;
|
||||
@ -254,6 +249,15 @@ class Curve3D : public Resource {
|
||||
|
||||
void _bake() const;
|
||||
|
||||
struct Interval {
|
||||
int idx;
|
||||
real_t frac;
|
||||
};
|
||||
Interval _find_interval(real_t p_offset) const;
|
||||
Vector3 _sample_baked(Interval p_interval, bool p_cubic) const;
|
||||
real_t _sample_baked_tilt(Interval p_interval) const;
|
||||
Basis _sample_posture(Interval p_interval, bool p_apply_tilt = false) const;
|
||||
|
||||
real_t bake_interval = 0.2;
|
||||
bool up_vector_enabled = true;
|
||||
|
||||
@ -296,9 +300,10 @@ public:
|
||||
|
||||
real_t get_baked_length() const;
|
||||
Vector3 sample_baked(real_t p_offset, bool p_cubic = false) const;
|
||||
Transform3D sample_baked_with_rotation(real_t p_offset, bool p_cubic = false, bool p_apply_tilt = false) const;
|
||||
real_t sample_baked_tilt(real_t p_offset) const;
|
||||
Vector3 sample_baked_up_vector(real_t p_offset, bool p_apply_tilt = false) const;
|
||||
PackedVector3Array get_baked_points() const; //useful for going through
|
||||
PackedVector3Array get_baked_points() const; // Useful for going through.
|
||||
Vector<real_t> get_baked_tilts() const; //useful for going through
|
||||
PackedVector3Array get_baked_up_vectors() const;
|
||||
Vector3 get_closest_point(const Vector3 &p_to_point) const;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user