diff options
| -rw-r--r-- | core/math/matrix3.h | 6 | ||||
| -rw-r--r-- | core/math/transform.cpp | 13 | ||||
| -rw-r--r-- | core/math/transform.h | 10 | ||||
| -rw-r--r-- | scene/3d/path.cpp | 66 |
4 files changed, 58 insertions, 37 deletions
diff --git a/core/math/matrix3.h b/core/math/matrix3.h index c3eeb1f705..8897c692f7 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -145,6 +145,12 @@ public: elements[2][1] = zy; elements[2][2] = zz; } + _FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { + + set_axis(0, p_x); + set_axis(1, p_y); + set_axis(2, p_z); + } _FORCE_INLINE_ Vector3 get_column(int i) const { return Vector3(elements[0][i], elements[1][i], elements[2][i]); diff --git a/core/math/transform.cpp b/core/math/transform.cpp index e53e6cf519..7a50214596 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -82,7 +82,10 @@ Transform Transform::looking_at(const Vector3 &p_target, const Vector3 &p_up) co } void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { - +#ifdef MATH_CHECKS + ERR_FAIL_COND(p_eye == p_target); + ERR_FAIL_COND(p_up.length() == 0); +#endif // Reference: MESA source code Vector3 v_x, v_y, v_z; @@ -96,6 +99,9 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const v_y = p_up; v_x = v_y.cross(v_z); +#ifdef MATH_CHECKS + ERR_FAIL_COND(v_x.length() == 0); +#endif /* Recompute Y = Z cross X */ v_y = v_z.cross(v_x); @@ -103,9 +109,8 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const v_x.normalize(); v_y.normalize(); - basis.set_axis(0, v_x); - basis.set_axis(1, v_y); - basis.set_axis(2, v_z); + basis.set(v_x, v_y, v_z); + origin = p_eye; } diff --git a/core/math/transform.h b/core/math/transform.h index 4731496bf3..bbb25a420a 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -97,15 +97,7 @@ public: void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t tx, real_t ty, real_t tz) { - basis.elements[0][0] = xx; - basis.elements[0][1] = xy; - basis.elements[0][2] = xz; - basis.elements[1][0] = yx; - basis.elements[1][1] = yy; - basis.elements[1][2] = yz; - basis.elements[2][0] = zx; - basis.elements[2][1] = zy; - basis.elements[2][2] = zz; + basis.set(xx, xy, xz, yx, yy, yz, zx, zy, zz); origin.x = tx; origin.y = ty; origin.z = tz; diff --git a/scene/3d/path.cpp b/scene/3d/path.cpp index d535c545a8..5c29918118 100644 --- a/scene/3d/path.cpp +++ b/scene/3d/path.cpp @@ -108,40 +108,58 @@ void PathFollow::_update_transform() { Vector3 pos = c->interpolate_baked(o, cubic); Transform t = get_transform(); - if (rotation_mode != ROTATION_NONE) { - - Vector3 n = (c->interpolate_baked(o + lookahead, cubic) - pos).normalized(); - - if (rotation_mode == ROTATION_Y) { + t.origin = pos; + Vector3 pos_offset = Vector3(h_offset, v_offset, 0); - n.y = 0; - n.normalize(); - } + 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. + + Vector3 t_prev = pos - c->interpolate_baked(o - lookahead, cubic); + Vector3 t_cur = c->interpolate_baked(o + lookahead, cubic) - pos; + + Vector3 axis = t_prev.cross(t_cur); + float dot = t_prev.normalized().dot(t_cur.normalized()); + float angle = Math::acos(CLAMP(dot, -1, 1)); + + if (axis.length() > CMP_EPSILON && angle > CMP_EPSILON) { + 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 OK + } - if (n.length() < CMP_EPSILON) { //nothing, use previous - n = -t.get_basis().get_axis(2).normalized(); + t.rotate_basis(axis.normalized(), angle); } - Vector3 up = Vector3(0, 1, 0); - - if (rotation_mode == ROTATION_XYZ) { - - float tilt = c->interpolate_baked_tilt(o); - if (tilt != 0) { - - Basis rot(-n, tilt); //remember.. lookat will be znegative.. znegative!! we abide by opengl clan. - up = rot.xform(up); + // do the additional tilting + float tilt_angle = c->interpolate_baked_tilt(o); + Vector3 tilt_axis = t_cur; // is this correct?? + + if (tilt_axis.length() > CMP_EPSILON && tilt_angle > CMP_EPSILON) { + 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 OK } - } - t.set_look_at(pos, pos + n, up); + t.rotate_basis(tilt_axis.normalized(), tilt_angle); + } + t.translate(pos_offset); } else { - - t.origin = pos; + t.origin += pos_offset; } - t.origin += t.basis.get_axis(0) * h_offset + t.basis.get_axis(1) * v_offset; set_transform(t); } |