summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/geometry.h15
-rw-r--r--core/math/math_2d.cpp10
-rw-r--r--core/math/math_2d.h11
-rw-r--r--core/math/matrix3.cpp13
-rw-r--r--core/math/matrix3.h2
-rw-r--r--core/math/quat.cpp16
-rw-r--r--core/math/quat.h24
-rw-r--r--core/math/quick_hull.cpp2
-rw-r--r--core/math/vector3.h16
9 files changed, 99 insertions, 10 deletions
diff --git a/core/math/geometry.h b/core/math/geometry.h
index 73a53c53b6..be998aef0b 100644
--- a/core/math/geometry.h
+++ b/core/math/geometry.h
@@ -529,6 +529,21 @@ public:
return p_segment[0] + n * d; // inside
}
+ static bool line_intersects_line_2d(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) {
+
+ // see http://paulbourke.net/geometry/pointlineplane/
+
+ const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y;
+ if (Math::abs(denom) < CMP_EPSILON) { // parallel?
+ return false;
+ }
+
+ const Vector2 v = p_from_a - p_from_b;
+ const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom;
+ r_result = p_from_a + t * p_dir_a;
+ return true;
+ }
+
static bool segment_intersects_segment_2d(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) {
Vector2 B = p_to_a - p_from_a;
diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp
index 3767d298a1..a053ffbd93 100644
--- a/core/math/math_2d.cpp
+++ b/core/math/math_2d.cpp
@@ -103,6 +103,16 @@ Vector2 Vector2::floor() const {
return Vector2(Math::floor(x), Math::floor(y));
}
+Vector2 Vector2::ceil() const {
+
+ return Vector2(Math::ceil(x), Math::ceil(y));
+}
+
+Vector2 Vector2::round() const {
+
+ return Vector2(Math::round(x), Math::round(y));
+}
+
Vector2 Vector2::rotated(real_t p_by) const {
Vector2 v;
diff --git a/core/math/math_2d.h b/core/math/math_2d.h
index e7188da85b..25c39e5d7a 100644
--- a/core/math/math_2d.h
+++ b/core/math/math_2d.h
@@ -112,6 +112,7 @@ struct Vector2 {
_FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t);
_FORCE_INLINE_ Vector2 linear_interpolate(const Vector2 &p_b, real_t p_t) const;
+ _FORCE_INLINE_ Vector2 slerp(const Vector2 &p_b, real_t p_t) const;
Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const;
Vector2 slide(const Vector2 &p_normal) const;
@@ -162,6 +163,8 @@ struct Vector2 {
}
Vector2 floor() const;
+ Vector2 ceil() const;
+ Vector2 round() const;
Vector2 snapped(const Vector2 &p_by) const;
real_t aspect() const { return width / height; }
@@ -261,6 +264,14 @@ Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const {
return res;
}
+Vector2 Vector2::slerp(const Vector2 &p_b, real_t p_t) const {
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Vector2());
+#endif
+ real_t theta = angle_to(p_b);
+ return rotated(theta * p_t);
+}
+
Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) {
Vector2 res = p_a;
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index b0b05d1ec8..8ee8ccb457 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -826,3 +826,16 @@ void Basis::set_diagonal(const Vector3 p_diag) {
elements[2][1] = 0;
elements[2][2] = p_diag.z;
}
+
+Basis Basis::slerp(const Basis &target, const real_t &t) const {
+ // TODO: implement this directly without using quaternions to make it more efficient
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_rotation() == false, Basis());
+ ERR_FAIL_COND_V(target.is_rotation() == false, Basis());
+#endif
+
+ Quat from(*this);
+ Quat to(target);
+
+ return Basis(from.slerp(to, t));
+}
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index fd383fc673..63d4f5d79d 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -155,6 +155,8 @@ public:
bool is_diagonal() const;
bool is_rotation() const;
+ Basis slerp(const Basis &target, const real_t &t) const;
+
operator String() const;
/* create / set */
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 4f61401ac7..b938fc3cfd 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -98,6 +98,9 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) {
// and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation).
Vector3 Quat::get_euler_yxz() const {
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Vector3(0, 0, 0));
+#endif
Basis m(*this);
return m.get_euler_yxz();
}
@@ -135,11 +138,17 @@ bool Quat::is_normalized() const {
}
Quat Quat::inverse() const {
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
+#endif
return Quat(-x, -y, -z, w);
}
Quat Quat::slerp(const Quat &q, const real_t &t) const {
-
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
+ ERR_FAIL_COND_V(q.is_normalized() == false, Quat(0, 0, 0, 0));
+#endif
Quat to1;
real_t omega, cosom, sinom, scale0, scale1;
@@ -215,7 +224,10 @@ Quat::operator String() const {
return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w);
}
-Quat::Quat(const Vector3 &axis, const real_t &angle) {
+void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) {
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND(axis.is_normalized() == false);
+#endif
real_t d = axis.length();
if (d == 0)
set(0, 0, 0, 0);
diff --git a/core/math/quat.h b/core/math/quat.h
index ebc924504b..3e1344a913 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -64,11 +64,13 @@ public:
Quat slerpni(const Quat &q, const real_t &t) const;
Quat cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const;
+ void set_axis_angle(const Vector3 &axis, const real_t &angle);
_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
r_angle = 2 * Math::acos(w);
- r_axis.x = x / Math::sqrt(1 - w * w);
- r_axis.y = y / Math::sqrt(1 - w * w);
- r_axis.z = z / Math::sqrt(1 - w * w);
+ real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
+ r_axis.x = x * r;
+ r_axis.y = y * r;
+ r_axis.z = z * r;
}
void operator*=(const Quat &q);
@@ -83,9 +85,9 @@ public:
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
- Quat q = *this * v;
- q *= this->inverse();
- return Vector3(q.x, q.y, q.z);
+ Vector3 u(x, y, z);
+ Vector3 uv = u.cross(v);
+ return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
}
_FORCE_INLINE_ void operator+=(const Quat &q);
@@ -115,7 +117,15 @@ public:
z = p_z;
w = p_w;
}
- Quat(const Vector3 &axis, const real_t &angle);
+ Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); }
+
+ Quat(const Vector3 &euler) { set_euler(euler); }
+ Quat(const Quat &q) {
+ x = q.x;
+ y = q.y;
+ z = q.z;
+ w = q.w;
+ }
Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc
{
diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp
index 102e454e02..fc90417413 100644
--- a/core/math/quick_hull.cpp
+++ b/core/math/quick_hull.cpp
@@ -74,7 +74,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me
int longest_axis = aabb.get_longest_axis_index();
//first two vertices are the most distant
- int simplex[4];
+ int simplex[4] = { 0 };
{
real_t max = 0, min = 0;
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 10ec4f5641..433adf09ee 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -91,6 +91,7 @@ struct Vector3 {
/* Static Methods between 2 vector3s */
_FORCE_INLINE_ Vector3 linear_interpolate(const Vector3 &p_b, real_t p_t) const;
+ _FORCE_INLINE_ Vector3 slerp(const Vector3 &p_b, real_t p_t) const;
Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const;
Vector3 cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const;
@@ -103,6 +104,7 @@ struct Vector3 {
_FORCE_INLINE_ Vector3 floor() const;
_FORCE_INLINE_ Vector3 sign() const;
_FORCE_INLINE_ Vector3 ceil() const;
+ _FORCE_INLINE_ Vector3 round() const;
_FORCE_INLINE_ real_t distance_to(const Vector3 &p_b) const;
_FORCE_INLINE_ real_t distance_squared_to(const Vector3 &p_b) const;
@@ -204,6 +206,11 @@ Vector3 Vector3::ceil() const {
return Vector3(Math::ceil(x), Math::ceil(y), Math::ceil(z));
}
+Vector3 Vector3::round() const {
+
+ return Vector3(Math::round(x), Math::round(y), Math::round(z));
+}
+
Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const {
return Vector3(
@@ -212,6 +219,15 @@ Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const {
z + (p_t * (p_b.z - z)));
}
+Vector3 Vector3::slerp(const Vector3 &p_b, real_t p_t) const {
+#ifdef MATH_CHECKS
+ ERR_FAIL_COND_V(is_normalized() == false, Vector3());
+#endif
+
+ real_t theta = angle_to(p_b);
+ return rotated(cross(p_b), theta * p_t);
+}
+
real_t Vector3::distance_to(const Vector3 &p_b) const {
return (p_b - *this).length();
generated by cgit v1.2.3 (git 2.25.1) at 2025-12-26 07:01:51 +0000