aboutsummaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/math_2d.cpp9
-rw-r--r--core/math/math_defs.h4
-rw-r--r--core/math/math_funcs.cpp6
-rw-r--r--core/math/math_funcs.h7
-rw-r--r--core/math/matrix3.cpp126
-rw-r--r--core/math/matrix3.h20
-rw-r--r--core/math/pcg.cpp15
-rw-r--r--core/math/pcg.h14
-rw-r--r--core/math/quat.cpp4
-rw-r--r--core/math/quat.h3
-rw-r--r--core/math/triangulator.cpp1550
-rw-r--r--core/math/triangulator.h306
-rw-r--r--core/math/vector3.h7
13 files changed, 138 insertions, 1933 deletions
diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp
index 20b916ee3..962a42acb 100644
--- a/core/math/math_2d.cpp
+++ b/core/math/math_2d.cpp
@@ -63,7 +63,8 @@ Vector2 Vector2::normalized() const {
}
bool Vector2::is_normalized() const {
- return Math::isequal_approx(length(), (real_t)1.0);
+ // use length_squared() instead of length() to avoid sqrt(), makes it more stringent.
+ return Math::is_equal_approx(length_squared(), 1.0);
}
real_t Vector2::distance_to(const Vector2 &p_vector2) const {
@@ -281,7 +282,7 @@ Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, c
// slide returns the component of the vector along the given plane, specified by its normal vector.
Vector2 Vector2::slide(const Vector2 &p_n) const {
-#ifdef DEBUG_ENABLED
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(p_n.is_normalized() == false, Vector2());
#endif
return *this - p_n * this->dot(p_n);
@@ -292,7 +293,7 @@ Vector2 Vector2::bounce(const Vector2 &p_n) const {
}
Vector2 Vector2::reflect(const Vector2 &p_n) const {
-#ifdef DEBUG_ENABLED
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(p_n.is_normalized() == false, Vector2());
#endif
return 2.0 * p_n * this->dot(p_n) - *this;
@@ -439,7 +440,9 @@ Transform2D Transform2D::inverse() const {
void Transform2D::affine_invert() {
real_t det = basis_determinant();
+#ifdef MATH_CHECKS
ERR_FAIL_COND(det == 0);
+#endif
real_t idet = 1.0 / det;
SWAP(elements[0][0], elements[1][1]);
diff --git a/core/math/math_defs.h b/core/math/math_defs.h
index 1a5768e51..3d9eb63e1 100644
--- a/core/math/math_defs.h
+++ b/core/math/math_defs.h
@@ -35,6 +35,10 @@
#define CMP_NORMALIZE_TOLERANCE 0.000001
#define CMP_POINT_IN_PLANE_EPSILON 0.00001
+#ifdef DEBUG_ENABLED
+#define MATH_CHECKS
+#endif
+
#define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0)
/**
* "Real" is a type that will be translated to either floats or fixed depending
diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp
index 6a46b9fbe..9f5a9c193 100644
--- a/core/math/math_funcs.cpp
+++ b/core/math/math_funcs.cpp
@@ -30,7 +30,7 @@
#include "math_funcs.h"
#include "core/os/os.h"
-pcg32_random_t Math::default_pcg = { 1, PCG_DEFAULT_INC_64 };
+pcg32_random_t Math::default_pcg = { 12047754176567800795ULL, PCG_DEFAULT_INC_64 };
#define PHI 0x9e3779b9
@@ -51,9 +51,7 @@ void Math::seed(uint64_t x) {
}
void Math::randomize() {
-
- OS::Time time = OS::get_singleton()->get_time();
- seed(OS::get_singleton()->get_ticks_usec() * (time.hour + 1) * (time.min + 1) * (time.sec + 1) * rand()); // TODO: can be simplified.
+ seed(OS::get_singleton()->get_ticks_usec() * default_pcg.state + PCG_DEFAULT_INC_64);
}
uint32_t Math::rand() {
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 10426c924..06ec77daa 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -31,9 +31,10 @@
#define MATH_FUNCS_H
#include "math_defs.h"
-#include "pcg.h"
#include "typedefs.h"
+#include "thirdparty/misc/pcg.h"
+
#include <float.h>
#include <math.h>
@@ -157,7 +158,7 @@ public:
static uint32_t larger_prime(uint32_t p_val);
- static void seed(uint64_t x = 0);
+ static void seed(uint64_t x);
static void randomize();
static uint32_t rand_from_seed(uint64_t *seed);
static uint32_t rand();
@@ -168,7 +169,7 @@ public:
static float random(float from, float to);
static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); }
- static _ALWAYS_INLINE_ bool isequal_approx(real_t a, real_t b) {
+ static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b) {
// TODO: Comparing floats for approximate-equality is non-trivial.
// Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators.
// A proper implementation in terms of ULPs should eventually replace the contents of this function.
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index ef368009d..c733251c3 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -62,8 +62,9 @@ void Basis::invert() {
real_t det = elements[0][0] * co[0] +
elements[0][1] * co[1] +
elements[0][2] * co[2];
-
+#ifdef MATH_CHECKS
ERR_FAIL_COND(det == 0);
+#endif
real_t s = 1.0 / det;
set(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
@@ -72,8 +73,9 @@ void Basis::invert() {
}
void Basis::orthonormalize() {
+#ifdef MATH_CHECKS
ERR_FAIL_COND(determinant() == 0);
-
+#endif
// Gram-Schmidt Process
Vector3 x = get_axis(0);
@@ -102,20 +104,20 @@ bool Basis::is_orthogonal() const {
Basis id;
Basis m = (*this) * transposed();
- return isequal_approx(id, m);
+ return is_equal_approx(id, m);
}
bool Basis::is_rotation() const {
- return Math::isequal_approx(determinant(), 1) && is_orthogonal();
+ return Math::is_equal_approx(determinant(), 1) && is_orthogonal();
}
bool Basis::is_symmetric() const {
- if (Math::abs(elements[0][1] - elements[1][0]) > CMP_EPSILON)
+ if (!Math::is_equal_approx(elements[0][1], elements[1][0]))
return false;
- if (Math::abs(elements[0][2] - elements[2][0]) > CMP_EPSILON)
+ if (!Math::is_equal_approx(elements[0][2], elements[2][0]))
return false;
- if (Math::abs(elements[1][2] - elements[2][1]) > CMP_EPSILON)
+ if (!Math::is_equal_approx(elements[1][2], elements[2][1]))
return false;
return true;
@@ -123,11 +125,11 @@ bool Basis::is_symmetric() const {
Basis Basis::diagonalize() {
- //NOTE: only implemented for symmetric matrices
- //with the Jacobi iterative method method
-
+//NOTE: only implemented for symmetric matrices
+//with the Jacobi iterative method method
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_symmetric(), Basis());
-
+#endif
const int ite_max = 1024;
real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2];
@@ -160,7 +162,7 @@ Basis Basis::diagonalize() {
// Compute the rotation angle
real_t angle;
- if (Math::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) {
+ if (Math::is_equal_approx(elements[j][j], elements[i][i])) {
angle = Math_PI / 4;
} else {
angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i]));
@@ -226,11 +228,25 @@ Basis Basis::scaled(const Vector3 &p_scale) const {
}
Vector3 Basis::get_scale() const {
- // We are assuming M = R.S, and performing a polar decomposition to extract R and S.
- // FIXME: We eventually need a proper polar decomposition.
- // As a cheap workaround until then, to ensure that R is a proper rotation matrix with determinant +1
- // (such that it can be represented by a Quat or Euler angles), we absorb the sign flip into the scaling matrix.
- // As such, it works in conjunction with get_rotation().
+ // FIXME: We are assuming M = R.S (R is rotation and S is scaling), and use polar decomposition to extract R and S.
+ // A polar decomposition is M = O.P, where O is an orthogonal matrix (meaning rotation and reflection) and
+ // P is a positive semi-definite matrix (meaning it contains absolute values of scaling along its diagonal).
+ //
+ // Despite being different from what we want to achieve, we can nevertheless make use of polar decomposition
+ // here as follows. We can split O into a rotation and a reflection as O = R.Q, and obtain M = R.S where
+ // we defined S = Q.P. Now, R is a proper rotation matrix and S is a (signed) scaling matrix,
+ // which can involve negative scalings. However, there is a catch: unlike the polar decomposition of M = O.P,
+ // the decomposition of O into a rotation and reflection matrix as O = R.Q is not unique.
+ // Therefore, we are going to do this decomposition by sticking to a particular convention.
+ // This may lead to confusion for some users though.
+ //
+ // The convention we use here is to absorb the sign flip into the scaling matrix.
+ // The same convention is also used in other similar functions such as set_scale,
+ // get_rotation_axis_angle, get_rotation, set_rotation_axis_angle, set_rotation_euler, ...
+ //
+ // A proper way to get rid of this issue would be to store the scaling values (or at least their signs)
+ // as a part of Basis. However, if we go that path, we need to disable direct (write) access to the
+ // matrix elements.
real_t det_sign = determinant() > 0 ? 1 : -1;
return det_sign * Vector3(
Vector3(elements[0][0], elements[1][0], elements[2][0]).length(),
@@ -238,6 +254,17 @@ Vector3 Basis::get_scale() const {
Vector3(elements[0][2], elements[1][2], elements[2][2]).length());
}
+// Sets scaling while preserving rotation.
+// This requires some care when working with matrices with negative determinant,
+// since we're using a particular convention for "polar" decomposition in get_scale and get_rotation.
+// For details, see the explanation in get_scale.
+void Basis::set_scale(const Vector3 &p_scale) {
+ Vector3 e = get_euler();
+ Basis(); // reset to identity
+ scale(p_scale);
+ rotate(e);
+}
+
// Multiplies the matrix from left by the rotation matrix: M -> R.M
// Note that this does *not* rotate the matrix itself.
//
@@ -260,6 +287,7 @@ void Basis::rotate(const Vector3 &p_euler) {
*this = rotated(p_euler);
}
+// TODO: rename this to get_rotation_euler
Vector3 Basis::get_rotation() const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
@@ -274,6 +302,42 @@ Vector3 Basis::get_rotation() const {
return m.get_euler();
}
+void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
+ // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
+ // and returns the Euler angles corresponding to the rotation part, complementing get_scale().
+ // See the comment in get_scale() for further information.
+ Basis m = orthonormalized();
+ real_t det = m.determinant();
+ if (det < 0) {
+ // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
+ m.scale(Vector3(-1, -1, -1));
+ }
+
+ m.get_axis_angle(p_axis, p_angle);
+}
+
+// Sets rotation while preserving scaling.
+// This requires some care when working with matrices with negative determinant,
+// since we're using a particular convention for "polar" decomposition in get_scale and get_rotation.
+// For details, see the explanation in get_scale.
+void Basis::set_rotation_euler(const Vector3 &p_euler) {
+ Vector3 s = get_scale();
+ Basis(); // reset to identity
+ scale(s);
+ rotate(p_euler);
+}
+
+// Sets rotation while preserving scaling.
+// This requires some care when working with matrices with negative determinant,
+// since we're using a particular convention for "polar" decomposition in get_scale and get_rotation.
+// For details, see the explanation in get_scale.
+void Basis::set_rotation_axis_angle(const Vector3 &p_axis, real_t p_angle) {
+ Vector3 s = get_scale();
+ Basis(); // reset to identity
+ scale(s);
+ rotate(p_axis, p_angle);
+}
+
// get_euler returns a vector containing the Euler angles in the format
// (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last
// (following the convention they are commonly defined in the literature).
@@ -294,9 +358,9 @@ Vector3 Basis::get_euler() const {
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
Vector3 euler;
-
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(is_rotation() == false, euler);
-
+#endif
euler.y = Math::asin(elements[0][2]);
if (euler.y < Math_PI * 0.5) {
if (euler.y > -Math_PI * 0.5) {
@@ -340,11 +404,11 @@ void Basis::set_euler(const Vector3 &p_euler) {
*this = xmat * (ymat * zmat);
}
-bool Basis::isequal_approx(const Basis &a, const Basis &b) const {
+bool Basis::is_equal_approx(const Basis &a, const Basis &b) const {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
- if (Math::isequal_approx(a.elements[i][j], b.elements[i][j]) == false)
+ if (Math::is_equal_approx(a.elements[i][j], b.elements[i][j]) == false)
return false;
}
}
@@ -387,8 +451,9 @@ Basis::operator String() const {
}
Basis::operator Quat() const {
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(is_rotation() == false, Quat());
-
+#endif
real_t trace = elements[0][0] + elements[1][1] + elements[2][2];
real_t temp[4];
@@ -482,9 +547,10 @@ void Basis::set_orthogonal_index(int p_index) {
*this = _ortho_bases[p_index];
}
-void Basis::get_axis_and_angle(Vector3 &r_axis, real_t &r_angle) const {
+void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
+#ifdef MATH_CHECKS
ERR_FAIL_COND(is_rotation() == false);
-
+#endif
real_t angle, x, y, z; // variables for result
real_t epsilon = 0.01; // margin to allow for rounding errors
real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
@@ -573,11 +639,11 @@ Basis::Basis(const Quat &p_quat) {
xz - wy, yz + wx, 1.0 - (xx + yy));
}
-Basis::Basis(const Vector3 &p_axis, real_t p_phi) {
- // Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
-
+void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
+// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle
+#ifdef MATH_CHECKS
ERR_FAIL_COND(p_axis.is_normalized() == false);
-
+#endif
Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z);
real_t cosine = Math::cos(p_phi);
@@ -595,3 +661,7 @@ Basis::Basis(const Vector3 &p_axis, real_t p_phi) {
elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine;
elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
}
+
+Basis::Basis(const Vector3 &p_axis, real_t p_phi) {
+ set_axis_angle(p_axis, p_phi);
+}
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 08e963f56..c3eeb1f70 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -77,15 +77,25 @@ public:
void rotate(const Vector3 &p_euler);
Basis rotated(const Vector3 &p_euler) const;
+
Vector3 get_rotation() const;
+ void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
- void scale(const Vector3 &p_scale);
- Basis scaled(const Vector3 &p_scale) const;
- Vector3 get_scale() const;
+ void set_rotation_euler(const Vector3 &p_euler);
+ void set_rotation_axis_angle(const Vector3 &p_axis, real_t p_angle);
Vector3 get_euler() const;
void set_euler(const Vector3 &p_euler);
+ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
+ void set_axis_angle(const Vector3 &p_axis, real_t p_phi);
+
+ void scale(const Vector3 &p_scale);
+ Basis scaled(const Vector3 &p_scale) const;
+
+ Vector3 get_scale() const;
+ void set_scale(const Vector3 &p_scale);
+
// transposed dot products
_FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {
return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2];
@@ -97,7 +107,7 @@ public:
return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
}
- bool isequal_approx(const Basis &a, const Basis &b) const;
+ bool is_equal_approx(const Basis &a, const Basis &b) const;
bool operator==(const Basis &p_matrix) const;
bool operator!=(const Basis &p_matrix) const;
@@ -121,8 +131,6 @@ public:
operator String() const;
- void get_axis_and_angle(Vector3 &r_axis, real_t &r_angle) const;
-
/* create / set */
_FORCE_INLINE_ 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) {
diff --git a/core/math/pcg.cpp b/core/math/pcg.cpp
deleted file mode 100644
index eac3b36d3..000000000
--- a/core/math/pcg.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-// *Really* minimal PCG32 code / (c) 2014 M.E. O'Neill / pcg-random.org
-// Licensed under Apache License 2.0 (NO WARRANTY, etc. see website)
-
-#include "pcg.h"
-
-uint32_t pcg32_random_r(pcg32_random_t* rng)
-{
- uint64_t oldstate = rng->state;
- // Advance internal state
- rng->state = oldstate * 6364136223846793005ULL + (rng->inc|1);
- // Calculate output function (XSH RR), uses old state for max ILP
- uint32_t xorshifted = ((oldstate >> 18u) ^ oldstate) >> 27u;
- uint32_t rot = oldstate >> 59u;
- return (xorshifted >> rot) | (xorshifted << ((-rot) & 31));
-}
diff --git a/core/math/pcg.h b/core/math/pcg.h
deleted file mode 100644
index 81f4c9770..000000000
--- a/core/math/pcg.h
+++ /dev/null
@@ -1,14 +0,0 @@
-// *Really* minimal PCG32 code / (c) 2014 M.E. O'Neill / pcg-random.org
-// Licensed under Apache License 2.0 (NO WARRANTY, etc. see website)
-
-#ifndef RANDOM_H
-#define RANDOM_H
-
-#include "typedefs.h"
-
-#define PCG_DEFAULT_INC_64 1442695040888963407ULL
-
-typedef struct { uint64_t state; uint64_t inc; } pcg32_random_t;
-uint32_t pcg32_random_r(pcg32_random_t* rng);
-
-#endif // RANDOM_H
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 966254222..0bea97c2e 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -92,6 +92,10 @@ Quat Quat::normalized() const {
return *this / length();
}
+bool Quat::is_normalized() const {
+ return Math::is_equal_approx(length(), 1.0);
+}
+
Quat Quat::inverse() const {
return Quat(-x, -y, -z, w);
}
diff --git a/core/math/quat.h b/core/math/quat.h
index 76b3cde2a..f22275b45 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -48,6 +48,7 @@ public:
real_t length() const;
void normalize();
Quat normalized() const;
+ bool is_normalized() const;
Quat inverse() const;
_FORCE_INLINE_ real_t dot(const Quat &q) const;
void set_euler(const Vector3 &p_euler);
@@ -56,7 +57,7 @@ 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;
- _FORCE_INLINE_ void get_axis_and_angle(Vector3 &r_axis, real_t &r_angle) const {
+ _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);
diff --git a/core/math/triangulator.cpp b/core/math/triangulator.cpp
deleted file mode 100644
index 75b2b064c..000000000
--- a/core/math/triangulator.cpp
+++ /dev/null
@@ -1,1550 +0,0 @@
-//Copyright (C) 2011 by Ivan Fratric
-//
-//Permission is hereby granted, free of charge, to any person obtaining a copy
-//of this software and associated documentation files (the "Software"), to deal
-//in the Software without restriction, including without limitation the rights
-//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-//copies of the Software, and to permit persons to whom the Software is
-//furnished to do so, subject to the following conditions:
-//
-//The above copyright notice and this permission notice shall be included in
-//all copies or substantial portions of the Software.
-//
-//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-//THE SOFTWARE.
-
-
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-
-#include "triangulator.h"
-
-
-#define TRIANGULATOR_VERTEXTYPE_REGULAR 0
-#define TRIANGULATOR_VERTEXTYPE_START 1
-#define TRIANGULATOR_VERTEXTYPE_END 2
-#define TRIANGULATOR_VERTEXTYPE_SPLIT 3
-#define TRIANGULATOR_VERTEXTYPE_MERGE 4
-
-TriangulatorPoly::TriangulatorPoly() {
- hole = false;
- numpoints = 0;
- points = NULL;
-}
-
-TriangulatorPoly::~TriangulatorPoly() {
- if(points) delete [] points;
-}
-
-void TriangulatorPoly::Clear() {
- if(points) delete [] points;
- hole = false;
- numpoints = 0;
- points = NULL;
-}
-
-void TriangulatorPoly::Init(long numpoints) {
- Clear();
- this->numpoints = numpoints;
- points = new Vector2[numpoints];
-}
-
-void TriangulatorPoly::Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3) {
- Init(3);
- points[0] = p1;
- points[1] = p2;
- points[2] = p3;
-}
-
-TriangulatorPoly::TriangulatorPoly(const TriangulatorPoly &src) {
- hole = src.hole;
- numpoints = src.numpoints;
- points = new Vector2[numpoints];
- memcpy(points, src.points, numpoints*sizeof(Vector2));
-}
-
-TriangulatorPoly& TriangulatorPoly::operator=(const TriangulatorPoly &src) {
- Clear();
- hole = src.hole;
- numpoints = src.numpoints;
- points = new Vector2[numpoints];
- memcpy(points, src.points, numpoints*sizeof(Vector2));
- return *this;
-}
-
-int TriangulatorPoly::GetOrientation() {
- long i1,i2;
- real_t area = 0;
- for(i1=0; i1<numpoints; i1++) {
- i2 = i1+1;
- if(i2 == numpoints) i2 = 0;
- area += points[i1].x * points[i2].y - points[i1].y * points[i2].x;
- }
- if(area>0) return TRIANGULATOR_CCW;
- if(area<0) return TRIANGULATOR_CW;
- return 0;
-}
-
-void TriangulatorPoly::SetOrientation(int orientation) {
- int polyorientation = GetOrientation();
- if(polyorientation&&(polyorientation!=orientation)) {
- Invert();
- }
-}
-
-void TriangulatorPoly::Invert() {
- long i;
- Vector2 *invpoints;
-
- invpoints = new Vector2[numpoints];
- for(i=0;i<numpoints;i++) {
- invpoints[i] = points[numpoints-i-1];
- }
-
- delete [] points;
- points = invpoints;
-}
-
-Vector2 TriangulatorPartition::Normalize(const Vector2 &p) {
- Vector2 r;
- real_t n = sqrt(p.x*p.x + p.y*p.y);
- if(n!=0) {
- r = p/n;
- } else {
- r.x = 0;
- r.y = 0;
- }
- return r;
-}
-
-real_t TriangulatorPartition::Distance(const Vector2 &p1, const Vector2 &p2) {
- real_t dx,dy;
- dx = p2.x - p1.x;
- dy = p2.y - p1.y;
- return(sqrt(dx*dx + dy*dy));
-}
-
-//checks if two lines intersect
-int TriangulatorPartition::Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22) {
- if((p11.x == p21.x)&&(p11.y == p21.y)) return 0;
- if((p11.x == p22.x)&&(p11.y == p22.y)) return 0;
- if((p12.x == p21.x)&&(p12.y == p21.y)) return 0;
- if((p12.x == p22.x)&&(p12.y == p22.y)) return 0;
-
- Vector2 v1ort,v2ort,v;
- real_t dot11,dot12,dot21,dot22;
-
- v1ort.x = p12.y-p11.y;
- v1ort.y = p11.x-p12.x;
-
- v2ort.x = p22.y-p21.y;
- v2ort.y = p21.x-p22.x;
-
- v = p21-p11;
- dot21 = v.x*v1ort.x + v.y*v1ort.y;
- v = p22-p11;
- dot22 = v.x*v1ort.x + v.y*v1ort.y;
-
- v = p11-p21;
- dot11 = v.x*v2ort.x + v.y*v2ort.y;
- v = p12-p21;
- dot12 = v.x*v2ort.x + v.y*v2ort.y;
-
- if(dot11*dot12>0) return 0;
- if(dot21*dot22>0) return 0;
-
- return 1;
-}
-
-//removes holes from inpolys by merging them with non-holes
-int TriangulatorPartition::RemoveHoles(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *outpolys) {
- List<TriangulatorPoly> polys;
- List<TriangulatorPoly>::Element *holeiter,*polyiter,*iter,*iter2;
- long i,i2,holepointindex,polypointindex;
- Vector2 holepoint,polypoint,bestpolypoint;
- Vector2 linep1,linep2;
- Vector2 v1,v2;
- TriangulatorPoly newpoly;
- bool hasholes;
- bool pointvisible;
- bool pointfound;
-
- //check for trivial case (no holes)
- hasholes = false;
- for(iter = inpolys->front(); iter; iter=iter->next()) {
- if(iter->get().IsHole()) {
- hasholes = true;
- break;
- }
- }
- if(!hasholes) {
- for(iter = inpolys->front(); iter; iter=iter->next()) {
- outpolys->push_back(iter->get());
- }
- return 1;
- }
-
- polys = *inpolys;
-
- while(1) {
- //find the hole point with the largest x
- hasholes = false;
- for(iter = polys.front(); iter; iter=iter->next()) {
- if(!iter->get().IsHole()) continue;
-
- if(!hasholes) {
- hasholes = true;
- holeiter = iter;
- holepointindex = 0;
- }
-
- for(i=0; i < iter->get().GetNumPoints(); i++) {
- if(iter->get().GetPoint(i).x > holeiter->get().GetPoint(holepointindex).x) {
- holeiter = iter;
- holepointindex = i;
- }
- }
- }
- if(!hasholes) break;
- holepoint = holeiter->get().GetPoint(holepointindex);
-
- pointfound = false;
- for(iter = polys.front(); iter; iter=iter->next()) {
- if(iter->get().IsHole()) continue;
- for(i=0; i < iter->get().GetNumPoints(); i++) {
- if(iter->get().GetPoint(i).x <= holepoint.x) continue;
- if(!InCone(iter->get().GetPoint((i+iter->get().GetNumPoints()-1)%(iter->get().GetNumPoints())),
- iter->get().GetPoint(i),
- iter->get().GetPoint((i+1)%(iter->get().GetNumPoints())),
- holepoint))
- continue;
- polypoint = iter->get().GetPoint(i);
- if(pointfound) {
- v1 = Normalize(polypoint-holepoint);
- v2 = Normalize(bestpolypoint-holepoint);
- if(v2.x > v1.x) continue;
- }
- pointvisible = true;
- for(iter2 = polys.front(); iter2; iter2=iter2->next()) {
- if(iter2->get().IsHole()) continue;
- for(i2=0; i2 < iter2->get().GetNumPoints(); i2++) {
- linep1 = iter2->get().GetPoint(i2);
- linep2 = iter2->get().GetPoint((i2+1)%(iter2->get().GetNumPoints()));
- if(Intersects(holepoint,polypoint,linep1,linep2)) {
- pointvisible = false;
- break;
- }
- }
- if(!pointvisible) break;
- }
- if(pointvisible) {
- pointfound = true;
- bestpolypoint = polypoint;
- polyiter = iter;
- polypointindex = i;
- }
- }
- }
-
- if(!pointfound) return 0;
-
- newpoly.Init(holeiter->get().GetNumPoints() + polyiter->get().GetNumPoints() + 2);
- i2 = 0;
- for(i=0;i<=polypointindex;i++) {
- newpoly[i2] = polyiter->get().GetPoint(i);
- i2++;
- }
- for(i=0;i<=holeiter->get().GetNumPoints();i++) {
- newpoly[i2] = holeiter->get().GetPoint((i+holepointindex)%holeiter->get().GetNumPoints());
- i2++;
- }
- for(i=polypointindex;i<polyiter->get().GetNumPoints();i++) {
- newpoly[i2] = polyiter->get().GetPoint(i);
- i2++;
- }
-
- polys.erase(holeiter);
- polys.erase(polyiter);
- polys.push_back(newpoly);
- }
-
- for(iter = polys.front(); iter; iter=iter->next()) {
- outpolys->push_back(iter->get());
- }
-
- return 1;
-}
-
-bool TriangulatorPartition::IsConvex(Vector2& p1, Vector2& p2, Vector2& p3) {
- real_t tmp;
- tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
- if(tmp>0) return 1;
- else return 0;
-}
-
-bool TriangulatorPartition::IsReflex(Vector2& p1, Vector2& p2, Vector2& p3) {
- real_t tmp;
- tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
- if(tmp<0) return 1;
- else return 0;
-}
-
-bool TriangulatorPartition::IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p) {
- if(IsConvex(p1,p,p2)) return false;
- if(IsConvex(p2,p,p3)) return false;
- if(IsConvex(p3,p,p1)) return false;
- return true;
-}
-
-bool TriangulatorPartition::InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p) {
- bool convex;
-
- convex = IsConvex(p1,p2,p3);
-
- if(convex) {
- if(!IsConvex(p1,p2,p)) return false;
- if(!IsConvex(p2,p3,p)) return false;
- return true;
- } else {
- if(IsConvex(p1,p2,p)) return true;
- if(IsConvex(p2,p3,p)) return true;
- return false;
- }
-}
-
-bool TriangulatorPartition::InCone(PartitionVertex *v, Vector2 &p) {
- Vector2 p1,p2,p3;
-
- p1 = v->previous->p;
- p2 = v->p;
- p3 = v->next->p;
-
- return InCone(p1,p2,p3,p);
-}
-
-void TriangulatorPartition::UpdateVertexReflexity(PartitionVertex *v) {
- PartitionVertex *v1,*v3;
- v1 = v->previous;
- v3 = v->next;
- v->isConvex = !IsReflex(v1->p,v->p,v3->p);
-}
-
-void TriangulatorPartition::UpdateVertex(PartitionVertex *v, PartitionVertex *vertices, long numvertices) {
- long i;
- PartitionVertex *v1,*v3;
- Vector2 vec1,vec3;
-
- v1 = v->previous;
- v3 = v->next;
-
- v->isConvex = IsConvex(v1->p,v->p,v3->p);
-
- vec1 = Normalize(v1->p - v->p);
- vec3 = Normalize(v3->p - v->p);
- v->angle = vec1.x*vec3.x + vec1.y*vec3.y;
-
- if(v->isConvex) {
- v->isEar = true;
- for(i=0;i<numvertices;i++) {
- if((vertices[i].p.x==v->p.x)&&(vertices[i].p.y==v->p.y)) continue;
- if((vertices[i].p.x==v1->p.x)&&(vertices[i].p.y==v1->p.y)) continue;
- if((vertices[i].p.x==v3->p.x)&&(vertices[i].p.y==v3->p.y)) continue;
- if(IsInside(v1->p,v->p,v3->p,vertices[i].p)) {
- v->isEar = false;
- break;
- }
- }
- } else {
- v->isEar = false;
- }
-}
-
-//triangulation by ear removal
-int TriangulatorPartition::Triangulate_EC(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) {
- long numvertices;
- PartitionVertex *vertices;
- PartitionVertex *ear;
- TriangulatorPoly triangle;
- long i,j;
- bool earfound;
-
- if(poly->GetNumPoints() < 3) return 0;
- if(poly->GetNumPoints() == 3) {
- triangles->push_back(*poly);
- return 1;
- }
-
- numvertices = poly->GetNumPoints();
-
- vertices = new PartitionVertex[numvertices];
- for(i=0;i<numvertices;i++) {
- vertices[i].isActive = true;
- vertices[i].p = poly->GetPoint(i);
- if(i==(numvertices-1)) vertices[i].next=&(vertices[0]);
- else vertices[i].next=&(vertices[i+1]);
- if(i==0) vertices[i].previous = &(vertices[numvertices-1]);
- else vertices[i].previous = &(vertices[i-1]);
- }
- for(i=0;i<numvertices;i++) {
- UpdateVertex(&vertices[i],vertices,numvertices);
- }
-
- for(i=0;i<numvertices-3;i++) {
- earfound = false;
- //find the most extruded ear
- for(j=0;j<numvertices;j++) {
- if(!vertices[j].isActive) continue;
- if(!vertices[j].isEar) continue;
- if(!earfound) {
- earfound = true;
- ear = &(vertices[j]);
- } else {
- if(vertices[j].angle > ear->angle) {
- ear = &(vertices[j]);
- }
- }
- }
- if(!earfound) {
- delete [] vertices;
- return 0;
- }
-
- triangle.Triangle(ear->previous->p,ear->p,ear->next->p);
- triangles->push_back(triangle);
-
- ear->isActive = false;
- ear->previous->next = ear->next;
- ear->next->previous = ear->previous;
-
- if(i==numvertices-4) break;
-
- UpdateVertex(ear->previous,vertices,numvertices);
- UpdateVertex(ear->next,vertices,numvertices);
- }
- for(i=0;i<numvertices;i++) {
- if(vertices[i].isActive) {
- triangle.Triangle(vertices[i].previous->p,vertices[i].p,vertices[i].next->p);
- triangles->push_back(triangle);
- break;
- }
- }
-
- delete [] vertices;
-
- return 1;
-}
-
-int TriangulatorPartition::Triangulate_EC(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles) {
- List<TriangulatorPoly> outpolys;
- List<TriangulatorPoly>::Element*iter;
-
- if(!RemoveHoles(inpolys,&outpolys)) return 0;
- for(iter=outpolys.front();iter;iter=iter->next()) {
- if(!Triangulate_EC(&(iter->get()),triangles)) return 0;
- }
- return 1;
-}
-
-int TriangulatorPartition::ConvexPartition_HM(TriangulatorPoly *poly, List<TriangulatorPoly> *parts) {
- List<TriangulatorPoly> triangles;
- List<TriangulatorPoly>::Element *iter1,*iter2;
- TriangulatorPoly *poly1,*poly2;
- TriangulatorPoly newpoly;
- Vector2 d1,d2,p1,p2,p3;
- long i11,i12,i21,i22,i13,i23,j,k;
- bool isdiagonal;
- long numreflex;
-
- //check if the poly is already convex
- numreflex = 0;
- for(i11=0;i11<poly->GetNumPoints();i11++) {
- if(i11==0) i12 = poly->GetNumPoints()-1;
- else i12=i11-1;
- if(i11==(poly->GetNumPoints()-1)) i13=0;
- else i13=i11+1;
- if(IsReflex(poly->GetPoint(i12),poly->GetPoint(i11),poly->GetPoint(i13))) {
- numreflex = 1;
- break;
- }
- }
- if(numreflex == 0) {
- parts->push_back(*poly);
- return 1;
- }
-
- if(!Triangulate_EC(poly,&triangles)) return 0;
-
- for(iter1 = triangles.front(); iter1 ; iter1=iter1->next()) {
- poly1 = &(iter1->get());
- for(i11=0;i11<poly1->GetNumPoints();i11++) {
- d1 = poly1->GetPoint(i11);
- i12 = (i11+1)%(poly1->GetNumPoints());
- d2 = poly1->GetPoint(i12);
-
- isdiagonal = false;
- for(iter2 = iter1; iter2 ; iter2=iter2->next()) {
- if(iter1 == iter2) continue;
- poly2 = &(iter2->get());
-
- for(i21=0;i21<poly2->GetNumPoints();i21++) {
- if((d2.x != poly2->GetPoint(i21).x)||(d2.y != poly2->GetPoint(i21).y)) continue;
- i22 = (i21+1)%(poly2->GetNumPoints());
- if((d1.x != poly2->GetPoint(i22).x)||(d1.y != poly2->GetPoint(i22).y)) continue;
- isdiagonal = true;
- break;
- }
- if(isdiagonal) break;
- }
-
- if(!isdiagonal) continue;
-
- p2 = poly1->GetPoint(i11);
- if(i11 == 0) i13 = poly1->GetNumPoints()-1;
- else i13 = i11-1;
- p1 = poly1->GetPoint(i13);
- if(i22 == (poly2->GetNumPoints()-1)) i23 = 0;
- else i23 = i22+1;
- p3 = poly2->GetPoint(i23);
-
- if(!IsConvex(p1,p2,p3)) continue;
-
- p2 = poly1->GetPoint(i12);
- if(i12 == (poly1->GetNumPoints()-1)) i13 = 0;
- else i13 = i12+1;
- p3 = poly1->GetPoint(i13);
- if(i21 == 0) i23 = poly2->GetNumPoints()-1;
- else i23 = i21-1;
- p1 = poly2->GetPoint(i23);
-
- if(!IsConvex(p1,p2,p3)) continue;
-
- newpoly.Init(poly1->GetNumPoints()+poly2->GetNumPoints()-2);
- k = 0;
- for(j=i12;j!=i11;j=(j+1)%(poly1->GetNumPoints())) {
- newpoly[k] = poly1->GetPoint(j);
- k++;
- }
- for(j=i22;j!=i21;j=(j+1)%(poly2->GetNumPoints())) {
- newpoly[k] = poly2->GetPoint(j);
- k++;
- }
-
- triangles.erase(iter2);
- iter1->get() = newpoly;
- poly1 = &(iter1->get());
- i11 = -1;
-
- continue;
- }
- }
-
- for(iter1 = triangles.front(); iter1 ; iter1=iter1->next()) {
- parts->push_back(iter1->get());
- }
-
- return 1;
-}
-
-int TriangulatorPartition::ConvexPartition_HM(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *parts) {
- List<TriangulatorPoly> outpolys;
- List<TriangulatorPoly>::Element* iter;
-
- if(!RemoveHoles(inpolys,&outpolys)) return 0;
- for(iter=outpolys.front();iter;iter=iter->next()) {
- if(!ConvexPartition_HM(&(iter->get()),parts)) return 0;
- }
- return 1;
-}
-
-//minimum-weight polygon triangulation by dynamic programming
-//O(n^3) time complexity
-//O(n^2) space complexity
-int TriangulatorPartition::Triangulate_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) {
- long i,j,k,gap,n;
- DPState **dpstates;
- Vector2 p1,p2,p3,p4;
- long bestvertex;
- real_t weight,minweight,d1,d2;
- Diagonal diagonal,newdiagonal;
- List<Diagonal> diagonals;
- TriangulatorPoly triangle;
- int ret = 1;
-
- n = poly->GetNumPoints();
- dpstates = new DPState *[n];
- for(i=1;i<n;i++) {
- dpstates[i] = new DPState[i];
- }
-
- //init states and visibility
- for(i=0;i<(n-1);i++) {
- p1 = poly->GetPoint(i);
- for(j=i+1;j<n;j++) {
- dpstates[j][i].visible = true;
- dpstates[j][i].weight = 0;
- dpstates[j][i].bestvertex = -1;
- if(j!=(i+1)) {
- p2 = poly->GetPoint(j);
-
- //visibility check
- if(i==0) p3 = poly->GetPoint(n-1);
- else p3 = poly->GetPoint(i-1);
- if(i==(n-1)) p4 = poly->GetPoint(0);
- else p4 = poly->GetPoint(i+1);
- if(!InCone(p3,p1,p4,p2)) {
- dpstates[j][i].visible = false;
- continue;
- }
-
- if(j==0) p3 = poly->GetPoint(n-1);
- else p3 = poly->GetPoint(j-1);
- if(j==(n-1)) p4 = poly->GetPoint(0);
- else p4 = poly->GetPoint(j+1);
- if(!InCone(p3,p2,p4,p1)) {
- dpstates[j][i].visible = false;
- continue;
- }
-
- for(k=0;k<n;k++) {
- p3 = poly->GetPoint(k);
- if(k==(n-1)) p4 = poly->GetPoint(0);
- else p4 = poly->GetPoint(k+1);
- if(Intersects(p1,p2,p3,p4)) {
- dpstates[j][i].visible = false;
- break;
- }
- }
- }
- }
- }
- dpstates[n-1][0].visible = true;
- dpstates[n-1][0].weight = 0;
- dpstates[n-1][0].bestvertex = -1;
-
- for(gap = 2; gap<n; gap++) {
- for(i=0; i<(n-gap); i++) {
- j = i+gap;
- if(!dpstates[j][i].visible) continue;
- bestvertex = -1;
- for(k=(i+1);k<j;k++) {
- if(!dpstates[k][i].visible) continue;
- if(!dpstates[j][k].visible) continue;
-
- if(k<=(i+1)) d1=0;
- else d1 = Distance(poly->GetPoint(i),poly->GetPoint(k));
- if(j<=(k+1)) d2=0;
- else d2 = Distance(poly->GetPoint(k),poly->GetPoint(j));
-
- weight = dpstates[k][i].weight + dpstates[j][k].weight + d1 + d2;
-
- if((bestvertex == -1)||(weight<minweight)) {
- bestvertex = k;
- minweight = weight;
- }
- }
- if(bestvertex == -1) {
- for(i=1;i<n;i++) {
- delete [] dpstates[i];
- }
- delete [] dpstates;
-
- return 0;
- }
-
- dpstates[j][i].bestvertex = bestvertex;
- dpstates[j][i].weight = minweight;
- }
- }
-
- newdiagonal.index1 = 0;
- newdiagonal.index2 = n-1;
- diagonals.push_back(newdiagonal);
- while(!diagonals.empty()) {
- diagonal = (diagonals.front()->get());
- diagonals.pop_front();
- bestvertex = dpstates[diagonal.index2][diagonal.index1].bestvertex;
- if(bestvertex == -1) {
- ret = 0;
- break;
- }
- triangle.Triangle(poly->GetPoint(diagonal.index1),poly->GetPoint(bestvertex),poly->GetPoint(diagonal.index2));
- triangles->push_back(triangle);
- if(bestvertex > (diagonal.index1+1)) {
- newdiagonal.index1 = diagonal.index1;
- newdiagonal.index2 = bestvertex;
- diagonals.push_back(newdiagonal);
- }
- if(diagonal.index2 > (bestvertex+1)) {
- newdiagonal.index1 = bestvertex;
- newdiagonal.index2 = diagonal.index2;
- diagonals.push_back(newdiagonal);
- }
- }
-
- for(i=1;i<n;i++) {
- delete [] dpstates[i];
- }
- delete [] dpstates;
-
- return ret;
-}
-
-void TriangulatorPartition::UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates) {
- Diagonal newdiagonal;
- List<Diagonal> *pairs;
- long w2;
-
- w2 = dpstates[a][b].weight;
- if(w>w2) return;
-
- pairs = &(dpstates[a][b].pairs);
- newdiagonal.index1 = i;
- newdiagonal.index2 = j;
-
- if(w<w2) {
- pairs->clear();
- pairs->push_front(newdiagonal);
- dpstates[a][b].weight = w;
- } else {
- if((!pairs->empty())&&(i <= pairs->front()->get().index1)) return;
- while((!pairs->empty())&&(pairs->front()->get().index2 >= j)) pairs->pop_front();
- pairs->push_front(newdiagonal);
- }
-}
-
-void TriangulatorPartition::TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) {
- List<Diagonal> *pairs;
- List<Diagonal>::Element *iter,*lastiter;
- long top;
- long w;
-
- if(!dpstates[i][j].visible) return;
- top = j;
- w = dpstates[i][j].weight;
- if(k-j > 1) {
- if (!dpstates[j][k].visible) return;
- w += dpstates[j][k].weight + 1;
- }
- if(j-i > 1) {
- pairs = &(dpstates[i][j].pairs);
- iter = NULL;
- lastiter = NULL;
- while(iter!=pairs->front()) {
- if (!iter)
- iter=pairs->back();
- else
- iter=iter->prev();
-
- if(!IsReflex(vertices[iter->get().index2].p,vertices[j].p,vertices[k].p)) lastiter = iter;
- else break;
- }
- if(lastiter == NULL) w++;
- else {
- if(IsReflex(vertices[k].p,vertices[i].p,vertices[lastiter->get().index1].p)) w++;
- else top = lastiter->get().index1;
- }
- }
- UpdateState(i,k,w,top,j,dpstates);
-}
-
-void TriangulatorPartition::TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) {
- List<Diagonal> *pairs;
- List<Diagonal>::Element* iter,*lastiter;
- long top;
- long w;
-
- if(!dpstates[j][k].visible) return;
- top = j;
- w = dpstates[j][k].weight;
-
- if (j-i > 1) {
- if (!dpstates[i][j].visible) return;
- w += dpstates[i][j].weight + 1;
- }
- if (k-j > 1) {
- pairs = &(dpstates[j][k].pairs);
-
- iter = pairs->front();
- if((!pairs->empty())&&(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->get().index1].p))) {
- lastiter = iter;
- while(iter!=NULL) {
- if(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->get().index1].p)) {
- lastiter = iter;
- iter=iter->next();
- }
- else break;
- }
- if(IsReflex(vertices[lastiter->get().index2].p,vertices[k].p,vertices[i].p)) w++;
- else top = lastiter->get().index2;
- } else w++;
- }
- UpdateState(i,k,w,j,top,dpstates);
-}
-
-int TriangulatorPartition::ConvexPartition_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *parts) {
- Vector2 p1,p2,p3,p4;
- PartitionVertex *vertices;
- DPState2 **dpstates;
- long i,j,k,n,gap;
- List<Diagonal> diagonals,diagonals2;
- Diagonal diagonal,newdiagonal;
- List<Diagonal> *pairs,*pairs2;
- List<Diagonal>::Element* iter,*iter2;
- int ret;
- TriangulatorPoly newpoly;
- List<long> indices;
- List<long>::Element* iiter;
- bool ijreal,jkreal;
-
- n = poly->GetNumPoints();
- vertices = new PartitionVertex[n];
-
- dpstates = new DPState2 *[n];
- for(i=0;i<n;i++) {
- dpstates[i] = new DPState2[n];
- }
-
- //init vertex information
- for(i=0;i<n;i++) {
- vertices[i].p = poly->GetPoint(i);
- vertices[i].isActive = true;
- if(i==0) vertices[i].previous = &(vertices[n-1]);
- else vertices[i].previous = &(vertices[i-1]);
- if(i==(poly->GetNumPoints()-1)) vertices[i].next = &(vertices[0]);
- else vertices[i].next = &(vertices[i+1]);
- }
- for(i=1;i<n;i++) {
- UpdateVertexReflexity(&(vertices[i]));
- }
-
- //init states and visibility
- for(i=0;i<(n-1);i++) {
- p1 = poly->GetPoint(i);
- for(j=i+1;j<n;j++) {
- dpstates[i][j].visible = true;
- if(j==i+1) {
- dpstates[i][j].weight = 0;
- } else {
- dpstates[i][j].weight = 2147483647;
- }
- if(j!=(i+1)) {
- p2 = poly->GetPoint(j);
-
- //visibility check
- if(!InCone(&vertices[i],p2)) {
- dpstates[i][j].visible = false;
- continue;
- }
- if(!InCone(&vertices[j],p1)) {
- dpstates[i][j].visible = false;
- continue;
- }
-
- for(k=0;k<n;k++) {
- p3 = poly->GetPoint(k);
- if(k==(n-1)) p4 = poly->GetPoint(0);
- else p4 = poly->GetPoint(k+1);
- if(Intersects(p1,p2,p3,p4)) {
- dpstates[i][j].visible = false;
- break;
- }
- }
- }
- }
- }
- for(i=0;i<(n-2);i++) {
- j = i+2;
- if(dpstates[i][j].visible) {
- dpstates[i][j].weight = 0;
- newdiagonal.index1 = i+1;
- newdiagonal.index2 = i+1;
- dpstates[i][j].pairs.push_back(newdiagonal);
- }
- }
-
- dpstates[0][n-1].visible = true;
- vertices[0].isConvex = false; //by convention
-
- for(gap=3; gap<n; gap++) {
- for(i=0;i<n-gap;i++) {
- if(vertices[i].isConvex) continue;
- k = i+gap;
- if(dpstates[i][k].visible) {
- if(!vertices[k].isConvex) {
- for(j=i+1;j<k;j++) TypeA(i,j,k,vertices,dpstates);
- } else {
- for(j=i+1;j<(k-1);j++) {
- if(vertices[j].isConvex) continue;
- TypeA(i,j,k,vertices,dpstates);
- }
- TypeA(i,k-1,k,vertices,dpstates);
- }
- }
- }
- for(k=gap;k<n;k++) {
- if(vertices[k].isConvex) continue;
- i = k-gap;
- if((vertices[i].isConvex)&&(dpstates[i][k].visible)) {
- TypeB(i,i+1,k,vertices,dpstates);
- for(j=i+2;j<k;j++) {
- if(vertices[j].isConvex) continue;
- TypeB(i,j,k,vertices,dpstates);
- }
- }
- }
- }
-
-
- //recover solution
- ret = 1;
- newdiagonal.index1 = 0;
- newdiagonal.index2 = n-1;
- diagonals.push_front(newdiagonal);
- while(!diagonals.empty()) {
- diagonal = (diagonals.front()->get());
- diagonals.pop_front();
- if((diagonal.index2 - diagonal.index1) <=1) continue;
- pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
- if(pairs->empty()) {
- ret = 0;
- break;
- }
- if(!vertices[diagonal.index1].isConvex) {
- iter = pairs->back();
-
- j = iter->get().index2;
- newdiagonal.index1 = j;
- newdiagonal.index2 = diagonal.index2;
- diagonals.push_front(newdiagonal);
- if((j - diagonal.index1)>1) {
- if(iter->get().index1 != iter->get().index2) {
- pairs2 = &(dpstates[diagonal.index1][j].pairs);
- while(1) {
- if(pairs2->empty()) {
- ret = 0;
- break;
- }
- iter2 = pairs2->back();
-
- if(iter->get().index1 != iter2->get().index1) pairs2->pop_back();
- else break;
- }
- if(ret == 0) break;
- }
- newdiagonal.index1 = diagonal.index1;
- newdiagonal.index2 = j;
- diagonals.push_front(newdiagonal);
- }
- } else {
- iter = pairs->front();
- j = iter->get().index1;
- newdiagonal.index1 = diagonal.index1;
- newdiagonal.index2 = j;
- diagonals.push_front(newdiagonal);
- if((diagonal.index2 - j) > 1) {
- if(iter->get().index1 != iter->get().index2) {
- pairs2 = &(dpstates[j][diagonal.index2].pairs);
- while(1) {
- if(pairs2->empty()) {
- ret = 0;
- break;
- }
- iter2 = pairs2->front();
- if(iter->get().index2 != iter2->get().index2) pairs2->pop_front();
- else break;
- }
- if(ret == 0) break;
- }
- newdiagonal.index1 = j;
- newdiagonal.index2 = diagonal.index2;
- diagonals.push_front(newdiagonal);
- }
- }
- }
-
- if(ret == 0) {
- for(i=0;i<n;i++) {
- delete [] dpstates[i];
- }
- delete [] dpstates;
- delete [] vertices;
-
- return ret;
- }
-
- newdiagonal.index1 = 0;
- newdiagonal.index2 = n-1;
- diagonals.push_front(newdiagonal);
- while(!diagonals.empty()) {
- diagonal = (diagonals.front())->get();
- diagonals.pop_front();
- if((diagonal.index2 - diagonal.index1) <= 1) continue;
-
- indices.clear();
- diagonals2.clear();
- indices.push_back(diagonal.index1);
- indices.push_back(diagonal.index2);
- diagonals2.push_front(diagonal);
-
- while(!diagonals2.empty()) {
- diagonal = (diagonals2.front()->get());
- diagonals2.pop_front();
- if((diagonal.index2 - diagonal.index1) <= 1) continue;
- ijreal = true;
- jkreal = true;
- pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
- if(!vertices[diagonal.index1].isConvex) {
- iter = pairs->back();
- j = iter->get().index2;
- if(iter->get().index1 != iter->get().index2) ijreal = false;
- } else {
- iter = pairs->front();
- j = iter->get().index1;
- if(iter->get().index1 != iter->get().index2) jkreal = false;
- }
-
- newdiagonal.index1 = diagonal.index1;
- newdiagonal.index2 = j;
- if(ijreal) {
- diagonals.push_back(newdiagonal);
- } else {
- diagonals2.push_back(newdiagonal);
- }
-
- newdiagonal.index1 = j;
- newdiagonal.index2 = diagonal.index2;
- if(jkreal) {
- diagonals.push_back(newdiagonal);
- } else {
- diagonals2.push_back(newdiagonal);
- }
-
- indices.push_back(j);
- }
-
- indices.sort();
- newpoly.Init((long)indices.size());
- k=0;
- for(iiter = indices.front();iiter;iiter=iiter->next()) {
- newpoly[k] = vertices[iiter->get()].p;
- k++;
- }
- parts->push_back(newpoly);
- }
-
- for(i=0;i<n;i++) {
- delete [] dpstates[i];
- }
- delete [] dpstates;
- delete [] vertices;
-
- return ret;
-}
-
-//triangulates a set of polygons by first partitioning them into monotone polygons
-//O(n*log(n)) time complexity, O(n) space complexity
-//the algorithm used here is outlined in the book
-//"Computational Geometry: Algorithms and Applications"
-//by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars
-int TriangulatorPartition::MonotonePartition(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *monotonePolys) {
- List<TriangulatorPoly>::Element *iter;
- MonotoneVertex *vertices;
- long i,numvertices,vindex,vindex2,newnumvertices,maxnumvertices;
- long polystartindex, polyendindex;
- TriangulatorPoly *poly;
- MonotoneVertex *v,*v2,*vprev,*vnext;
- ScanLineEdge newedge;
- bool error = false;
-
- numvertices = 0;
- for(iter = inpolys->front(); iter ; iter=iter->next()) {
- numvertices += iter->get().GetNumPoints();
- }
-
- maxnumvertices = numvertices*3;
- vertices = new MonotoneVertex[maxnumvertices];
- newnumvertices = numvertices;
-
- polystartindex = 0;
- for(iter = inpolys->front(); iter ; iter=iter->next()) {
- poly = &(iter->get());
- polyendindex = polystartindex + poly->GetNumPoints()-1;
- for(i=0;i<poly->GetNumPoints();i++) {
- vertices[i+polystartindex].p = poly->GetPoint(i);
- if(i==0) vertices[i+polystartindex].previous = polyendindex;
- else vertices[i+polystartindex].previous = i+polystartindex-1;
- if(i==(poly->GetNumPoints()-1)) vertices[i+polystartindex].next = polystartindex;
- else vertices[i+polystartindex].next = i+polystartindex+1;
- }
- polystartindex = polyendindex+1;
- }
-
- //construct the priority queue
- long *priority = new long [numvertices];
- for(i=0;i<numvertices;i++) priority[i] = i;
- SortArray<long,VertexSorter> sorter;
- sorter.compare.vertices=vertices;
- sorter.sort(priority,numvertices);
-
- //determine vertex types
- char *vertextypes = new char[maxnumvertices];
- for(i=0;i<numvertices;i++) {
- v = &(vertices[i]);
- vprev = &(vertices[v->previous]);
- vnext = &(vertices[v->next]);
-
- if(Below(vprev->p,v->p)&&Below(vnext->p,v->p)) {
- if(IsConvex(vnext->p,vprev->p,v->p)) {
- vertextypes[i] = TRIANGULATOR_VERTEXTYPE_START;
- } else {
- vertextypes[i] = TRIANGULATOR_VERTEXTYPE_SPLIT;
- }
- } else if(Below(v->p,vprev->p)&&Below(v->p,vnext->p)) {
- if(IsConvex(vnext->p,vprev->p,v->p))
- {
- vertextypes[i] = TRIANGULATOR_VERTEXTYPE_END;
- } else {
- vertextypes[i] = TRIANGULATOR_VERTEXTYPE_MERGE;
- }
- } else {
- vertextypes[i] = TRIANGULATOR_VERTEXTYPE_REGULAR;
- }
- }
-
- //helpers
- long *helpers = new long[maxnumvertices];
-
- //binary search tree that holds edges intersecting the scanline
- //note that while set doesn't actually have to be implemented as a tree
- //complexity requirements for operations are the same as for the balanced binary search tree
- Set<ScanLineEdge> edgeTree;
- //store iterators to the edge tree elements
- //this makes deleting existing edges much faster
- Set<ScanLineEdge>::Element **edgeTreeIterators,*edgeIter;
- edgeTreeIterators = new Set<ScanLineEdge>::Element*[maxnumvertices];
- //Pair<Set<ScanLineEdge>::Element*,bool> edgeTreeRet;
- for(i = 0; i<numvertices; i++) edgeTreeIterators[i] = NULL;
-
- //for each vertex
- for(i=0;i<numvertices;i++) {
- vindex = priority[i];
- v = &(vertices[vindex]);
- vindex2 = vindex;
- v2 = v;
-
- //depending on the vertex type, do the appropriate action
- //comments in the following sections are copied from "Computational Geometry: Algorithms and Applications"
- switch(vertextypes[vindex]) {
- case TRIANGULATOR_VERTEXTYPE_START:
- //Insert ei in T and set helper(ei) to vi.
- newedge.p1 = v->p;
- newedge.p2 = vertices[v->next].p;
- newedge.index = vindex;
- edgeTreeIterators[vindex] = edgeTree.insert(newedge);
- helpers[vindex] = vindex;
- break;
-
- case TRIANGULATOR_VERTEXTYPE_END:
- //if helper(ei-1) is a merge vertex
- if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
- //Insert the diagonal connecting vi to helper(ei-1) in D.
- AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- }
- //Delete ei-1 from T
- edgeTree.erase(edgeTreeIterators[v->previous]);
- break;
-
- case TRIANGULATOR_VERTEXTYPE_SPLIT:
- //Search in T to find the edge e j directly left of vi.
- newedge.p1 = v->p;
- newedge.p2 = v->p;
- edgeIter = edgeTree.lower_bound(newedge);
- if(edgeIter == edgeTree.front()) {
- error = true;
- break;
- }
- edgeIter=edgeIter->prev();
- //Insert the diagonal connecting vi to helper(ej) in D.
- AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->get().index],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- vindex2 = newnumvertices-2;
- v2 = &(vertices[vindex2]);
- //helper(e j)�vi
- helpers[edgeIter->get().index] = vindex;
- //Insert ei in T and set helper(ei) to vi.
- newedge.p1 = v2->p;
- newedge.p2 = vertices[v2->next].p;
- newedge.index = vindex2;
-
- edgeTreeIterators[vindex2] = edgeTree.insert(newedge);
- helpers[vindex2] = vindex2;
- break;
-
- case TRIANGULATOR_VERTEXTYPE_MERGE:
- //if helper(ei-1) is a merge vertex
- if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
- //Insert the diagonal connecting vi to helper(ei-1) in D.
- AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- vindex2 = newnumvertices-2;
- v2 = &(vertices[vindex2]);
- }
- //Delete ei-1 from T.
- edgeTree.erase(edgeTreeIterators[v->previous]);
- //Search in T to find the edge e j directly left of vi.
- newedge.p1 = v->p;
- newedge.p2 = v->p;
- edgeIter = edgeTree.lower_bound(newedge);
- if(edgeIter == edgeTree.front()) {
- error = true;
- break;
- }
- edgeIter=edgeIter->prev();
- //if helper(ej) is a merge vertex
- if(vertextypes[helpers[edgeIter->get().index]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
- //Insert the diagonal connecting vi to helper(e j) in D.
- AddDiagonal(vertices,&newnumvertices,vindex2,helpers[edgeIter->get().index],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- }
- //helper(e j)�vi
- helpers[edgeIter->get().index] = vindex2;
- break;
-
- case TRIANGULATOR_VERTEXTYPE_REGULAR:
- //if the interior of P lies to the right of vi
- if(Below(v->p,vertices[v->previous].p)) {
- //if helper(ei-1) is a merge vertex
- if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
- //Insert the diagonal connecting vi to helper(ei-1) in D.
- AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- vindex2 = newnumvertices-2;
- v2 = &(vertices[vindex2]);
- }
- //Delete ei-1 from T.
- edgeTree.erase(edgeTreeIterators[v->previous]);
- //Insert ei in T and set helper(ei) to vi.
- newedge.p1 = v2->p;
- newedge.p2 = vertices[v2->next].p;
- newedge.index = vindex2;
- edgeTreeIterators[vindex2] = edgeTree.insert(newedge);
- helpers[vindex2] = vindex;
- } else {
- //Search in T to find the edge ej directly left of vi.
- newedge.p1 = v->p;
- newedge.p2 = v->p;
- edgeIter = edgeTree.lower_bound(newedge);
- if(edgeIter == edgeTree.front()) {
- error = true;
- break;
- }
- edgeIter=edgeIter->prev();
- //if helper(ej) is a merge vertex
- if(vertextypes[helpers[edgeIter->get().index]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
- //Insert the diagonal connecting vi to helper(e j) in D.
- AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->get().index],
- vertextypes, edgeTreeIterators, &edgeTree, helpers);
- }
- //helper(e j)�vi
- helpers[edgeIter->get().index] = vindex;
- }
- break;
- }
-
- if(error) break;
- }
-
- char *used = new char[newnumvertices];
- memset(used,0,newnumvertices*sizeof(char));
-
- if(!error) {
- //return result
- long size;
- TriangulatorPoly mpoly;
- for(i=0;i<newnumvertices;i++) {
- if(used[i]) continue;
- v = &(vertices[i]);
- vnext = &(vertices[v->next]);
- size = 1;
- while(vnext!=v) {
- vnext = &(vertices[vnext->next]);
- size++;
- }
- mpoly.Init(size);
- v = &(vertices[i]);
- mpoly[0] = v->p;
- vnext = &(vertices[v->next]);
- size = 1;
- used[i] = 1;
- used[v->next] = 1;
- while(vnext!=v) {
- mpoly[size] = vnext->p;
- used[vnext->next] = 1;
- vnext = &(vertices[vnext->next]);
- size++;
- }
- monotonePolys->push_back(mpoly);
- }
- }
-
- //cleanup
- delete [] vertices;
- delete [] priority;
- delete [] vertextypes;
- delete [] edgeTreeIterators;
- delete [] helpers;
- delete [] used;
-
- if(error) {
- return 0;
- } else {
- return 1;
- }
-}
-
-//adds a diagonal to the doubly-connected list of vertices
-void TriangulatorPartition::AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2,
- char *vertextypes, Set<ScanLineEdge>::Element **edgeTreeIterators,
- Set<ScanLineEdge> *edgeTree, long *helpers)
-{
- long newindex1,newindex2;
-
- newindex1 = *numvertices;
- (*numvertices)++;
- newindex2 = *numvertices;
- (*numvertices)++;
-
- vertices[newindex1].p = vertices[index1].p;
- vertices[newindex2].p = vertices[index2].p;
-
- vertices[newindex2].next = vertices[index2].next;
- vertices[newindex1].next = vertices[index1].next;
-
- vertices[vertices[index2].next].previous = newindex2;
- vertices[vertices[index1].next].previous = newindex1;
-
- vertices[index1].next = newindex2;
- vertices[newindex2].previous = index1;
-
- vertices[index2].next = newindex1;
- vertices[newindex1].previous = index2;
-
- //update all relevant structures
- vertextypes[newindex1] = vertextypes[index1];
- edgeTreeIterators[newindex1] = edgeTreeIterators[index1];
- helpers[newindex1] = helpers[index1];
- if(edgeTreeIterators[newindex1] != NULL)
- edgeTreeIterators[newindex1]->get().index = newindex1;
- vertextypes[newindex2] = vertextypes[index2];
- edgeTreeIterators[newindex2] = edgeTreeIterators[index2];
- helpers[newindex2] = helpers[index2];
- if(edgeTreeIterators[newindex2] != NULL)
- edgeTreeIterators[newindex2]->get().index = newindex2;
-}
-
-bool TriangulatorPartition::Below(Vector2 &p1, Vector2 &p2) {
- if(p1.y < p2.y) return true;
- else if(p1.y == p2.y) {
- if(p1.x < p2.x) return true;
- }
- return false;
-}
-
-
-
-
-
-//sorts in the falling order of y values, if y is equal, x is used instead
-bool TriangulatorPartition::VertexSorter::operator() (long index1, long index2) const {
- if(vertices[index1].p.y > vertices[index2].p.y) return true;
- else if(vertices[index1].p.y == vertices[index2].p.y) {
- if(vertices[index1].p.x > vertices[index2].p.x) return true;
- }
- return false;
-}
-
-bool TriangulatorPartition::ScanLineEdge::IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const {
- real_t tmp;
- tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
- if(tmp>0) return 1;
- else return 0;
-}
-
-bool TriangulatorPartition::ScanLineEdge::operator < (const ScanLineEdge & other) const {
- if(other.p1.y == other.p2.y) {
- if(p1.y == p2.y) {
- if(p1.y < other.p1.y) return true;
- else return false;
- }
- if(IsConvex(p1,p2,other.p1)) return true;
- else return false;
- } else if(p1.y == p2.y) {
- if(IsConvex(other.p1,other.p2,p1)) return false;
- else return true;
- } else if(p1.y < other.p1.y) {
- if(IsConvex(other.p1,other.p2,p1)) return false;
- else return true;
- } else {
- if(IsConvex(p1,p2,other.p1)) return true;
- else return false;
- }
-}
-
-//triangulates monotone polygon
-//O(n) time, O(n) space complexity
-int TriangulatorPartition::TriangulateMonotone(TriangulatorPoly *inPoly, List<TriangulatorPoly> *triangles) {
- long i,i2,j,topindex,bottomindex,leftindex,rightindex,vindex;
- Vector2 *points;
- long numpoints;
- TriangulatorPoly triangle;
-
- numpoints = inPoly->GetNumPoints();
- points = inPoly->GetPoints();
-
- //trivial calses
- if(numpoints < 3) return 0;
- if(numpoints == 3) {
- triangles->push_back(*inPoly);
- }
-
- topindex = 0; bottomindex=0;
- for(i=1;i<numpoints;i++) {
- if(Below(points[i],points[bottomindex])) bottomindex = i;
- if(Below(points[topindex],points[i])) topindex = i;
- }
-
- //check if the poly is really monotone
- i = topindex;
- while(i!=bottomindex) {
- i2 = i+1; if(i2>=numpoints) i2 = 0;
- if(!Below(points[i2],points[i])) return 0;
- i = i2;
- }
- i = bottomindex;
- while(i!=topindex) {
- i2 = i+1; if(i2>=numpoints) i2 = 0;
- if(!Below(points[i],points[i2])) return 0;
- i = i2;
- }
-
- char *vertextypes = new char[numpoints];
- long *priority = new long[numpoints];
-
- //merge left and right vertex chains
- priority[0] = topindex;
- vertextypes[topindex] = 0;
- leftindex = topindex+1; if(leftindex>=numpoints) leftindex = 0;
- rightindex = topindex-1; if(rightindex<0) rightindex = numpoints-1;
- for(i=1;i<(numpoints-1);i++) {
- if(leftindex==bottomindex) {
- priority[i] = rightindex;
- rightindex--; if(rightindex<0) rightindex = numpoints-1;
- vertextypes[priority[i]] = -1;
- } else if(rightindex==bottomindex) {
- priority[i] = leftindex;
- leftindex++; if(leftindex>=numpoints) leftindex = 0;
- vertextypes[priority[i]] = 1;
- } else {
- if(Below(points[leftindex],points[rightindex])) {
- priority[i] = rightindex;
- rightindex--; if(rightindex<0) rightindex = numpoints-1;
- vertextypes[priority[i]] = -1;
- } else {
- priority[i] = leftindex;
- leftindex++; if(leftindex>=numpoints) leftindex = 0;
- vertextypes[priority[i]] = 1;
- }
- }
- }
- priority[i] = bottomindex;
- vertextypes[bottomindex] = 0;
-
- long *stack = new long[numpoints];
- long stackptr = 0;
-
- stack[0] = priority[0];
- stack[1] = priority[1];
- stackptr = 2;
-
- //for each vertex from top to bottom trim as many triangles as possible
- for(i=2;i<(numpoints-1);i++) {
- vindex = priority[i];
- if(vertextypes[vindex]!=vertextypes[stack[stackptr-1]]) {
- for(j=0;j<(stackptr-1);j++) {
- if(vertextypes[vindex]==1) {
- triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
- } else {
- triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
- }
- triangles->push_back(triangle);
- }
- stack[0] = priority[i-1];
- stack[1] = priority[i];
- stackptr = 2;
- } else {
- stackptr--;
- while(stackptr>0) {
- if(vertextypes[vindex]==1) {
- if(IsConvex(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]])) {
- triangle.Triangle(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]]);
- triangles->push_back(triangle);
- stackptr--;
- } else {
- break;
- }
- } else {
- if(IsConvex(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]])) {
- triangle.Triangle(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]]);
- triangles->push_back(triangle);
- stackptr--;
- } else {
- break;
- }
- }
- }
- stackptr++;
- stack[stackptr] = vindex;
- stackptr++;
- }
- }
- vindex = priority[i];
- for(j=0;j<(stackptr-1);j++) {
- if(vertextypes[stack[j+1]]==1) {
- triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
- } else {
- triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
- }
- triangles->push_back(triangle);
- }
-
- delete [] priority;
- delete [] vertextypes;
- delete [] stack;
-
- return 1;
-}
-
-int TriangulatorPartition::Triangulate_MONO(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles) {
- List<TriangulatorPoly> monotone;
- List<TriangulatorPoly>::Element* iter;
-
- if(!MonotonePartition(inpolys,&monotone)) return 0;
- for(iter = monotone.front(); iter;iter=iter->next()) {
- if(!TriangulateMonotone(&(iter->get()),triangles)) return 0;
- }
- return 1;
-}
-
-int TriangulatorPartition::Triangulate_MONO(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) {
- List<TriangulatorPoly> polys;
- polys.push_back(*poly);
-
- return Triangulate_MONO(&polys, triangles);
-}
diff --git a/core/math/triangulator.h b/core/math/triangulator.h
deleted file mode 100644
index b6dd7e823..000000000
--- a/core/math/triangulator.h
+++ /dev/null
@@ -1,306 +0,0 @@
-//Copyright (C) 2011 by Ivan Fratric
-//
-//Permission is hereby granted, free of charge, to any person obtaining a copy
-//of this software and associated documentation files (the "Software"), to deal
-//in the Software without restriction, including without limitation the rights
-//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-//copies of the Software, and to permit persons to whom the Software is
-//furnished to do so, subject to the following conditions:
-//
-//The above copyright notice and this permission notice shall be included in
-//all copies or substantial portions of the Software.
-//
-//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-//THE SOFTWARE.
-
-#ifndef TRIANGULATOR_H
-#define TRIANGULATOR_H
-
-#include "math_2d.h"
-#include "list.h"
-#include "set.h"
-//2D point structure
-
-
-#define TRIANGULATOR_CCW 1
-#define TRIANGULATOR_CW -1
-//Polygon implemented as an array of points with a 'hole' flag
-class TriangulatorPoly {
-protected:
-
-
-
- Vector2 *points;
- long numpoints;
- bool hole;
-
-public:
-
- //constructors/destructors
- TriangulatorPoly();
- ~TriangulatorPoly();
-
- TriangulatorPoly(const TriangulatorPoly &src);
- TriangulatorPoly& operator=(const TriangulatorPoly &src);
-
- //getters and setters
- long GetNumPoints() {
- return numpoints;
- }
-
- bool IsHole() {
- return hole;
- }
-
- void SetHole(bool hole) {
- this->hole = hole;
- }
-
- Vector2 &GetPoint(long i) {
- return points[i];
- }
-
- Vector2 *GetPoints() {
- return points;
- }
-
- Vector2& operator[] (int i) {
- return points[i];
- }
-
- //clears the polygon points
- void Clear();
-
- //inits the polygon with numpoints vertices
- void Init(long numpoints);
-
- //creates a triangle with points p1,p2,p3
- void Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3);
-
- //inverts the orfer of vertices
- void Invert();
-
- //returns the orientation of the polygon
- //possible values:
- // Triangulator_CCW : polygon vertices are in counter-clockwise order
- // Triangulator_CW : polygon vertices are in clockwise order
- // 0 : the polygon has no (measurable) area
- int GetOrientation();
-
- //sets the polygon orientation
- //orientation can be
- // Triangulator_CCW : sets vertices in counter-clockwise order
- // Triangulator_CW : sets vertices in clockwise order
- void SetOrientation(int orientation);
-};
-
-class TriangulatorPartition {
-protected:
- struct PartitionVertex {
- bool isActive;
- bool isConvex;
- bool isEar;
-
- Vector2 p;
- real_t angle;
- PartitionVertex *previous;
- PartitionVertex *next;
- };
-
- struct MonotoneVertex {
- Vector2 p;
- long previous;
- long next;
- };
-
- struct VertexSorter{
- mutable MonotoneVertex *vertices;
- bool operator() (long index1, long index2) const;
- };
-
- struct Diagonal {
- long index1;
- long index2;
- };
-
- //dynamic programming state for minimum-weight triangulation
- struct DPState {
- bool visible;
- real_t weight;
- long bestvertex;
- };
-
- //dynamic programming state for convex partitioning
- struct DPState2 {
- bool visible;
- long weight;
- List<Diagonal> pairs;
- };
-
- //edge that intersects the scanline
- struct ScanLineEdge {
- mutable long index;
- Vector2 p1;
- Vector2 p2;
-
- //determines if the edge is to the left of another edge
- bool operator< (const ScanLineEdge & other) const;
-
- bool IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const;
- };
-
- //standard helper functions
- bool IsConvex(Vector2& p1, Vector2& p2, Vector2& p3);
- bool IsReflex(Vector2& p1, Vector2& p2, Vector2& p3);
- bool IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p);
-
- bool InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p);
- bool InCone(PartitionVertex *v, Vector2 &p);
-
- int Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22);
-
- Vector2 Normalize(const Vector2 &p);
- real_t Distance(const Vector2 &p1, const Vector2 &p2);
-
- //helper functions for Triangulate_EC
- void UpdateVertexReflexity(PartitionVertex *v);
- void UpdateVertex(PartitionVertex *v,PartitionVertex *vertices, long numvertices);
-
- //helper functions for ConvexPartition_OPT
- void UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates);
- void TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates);
- void TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates);
-
- //helper functions for MonotonePartition
- bool Below(Vector2 &p1, Vector2 &p2);
- void AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2,
- char *vertextypes, Set<ScanLineEdge>::Element **edgeTreeIterators,
- Set<ScanLineEdge> *edgeTree, long *helpers);
-
- //triangulates a monotone polygon, used in Triangulate_MONO
- int TriangulateMonotone(TriangulatorPoly *inPoly, List<TriangulatorPoly> *triangles);
-
-public:
-
- //simple heuristic procedure for removing holes from a list of polygons
- //works by creating a diagonal from the rightmost hole vertex to some visible vertex
- //time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices
- //space complexity: O(n)
- //params:
- // inpolys : a list of polygons that can contain holes
- // vertices of all non-hole polys have to be in counter-clockwise order
- // vertices of all hole polys have to be in clockwise order
- // outpolys : a list of polygons without holes
- //returns 1 on success, 0 on failure
- int RemoveHoles(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *outpolys);
-
- //triangulates a polygon by ear clipping
- //time complexity O(n^2), n is the number of vertices
- //space complexity: O(n)
- //params:
- // poly : an input polygon to be triangulated
- // vertices have to be in counter-clockwise order
- // triangles : a list of triangles (result)
- //returns 1 on success, 0 on failure
- int Triangulate_EC(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles);
-
- //triangulates a list of polygons that may contain holes by ear clipping algorithm
- //first calls RemoveHoles to get rid of the holes, and then Triangulate_EC for each resulting polygon
- //time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices
- //space complexity: O(n)
- //params:
- // inpolys : a list of polygons to be triangulated (can contain holes)
- // vertices of all non-hole polys have to be in counter-clockwise order
- // vertices of all hole polys have to be in clockwise order
- // triangles : a list of triangles (result)
- //returns 1 on success, 0 on failure
- int Triangulate_EC(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles);
-
- //creates an optimal polygon triangulation in terms of minimal edge length
- //time complexity: O(n^3), n is the number of vertices
- //space complexity: O(n^2)
- //params:
- // poly : an input polygon to be triangulated
- // vertices have to be in counter-clockwise order
- // triangles : a list of triangles (result)
- //returns 1 on success, 0 on failure
- int Triangulate_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles);
-
- //triangulates a polygons by firstly partitioning it into monotone polygons
- //time complexity: O(n*log(n)), n is the number of vertices
- //space complexity: O(n)
- //params:
- // poly : an input polygon to be triangulated
- // vertices have to be in counter-clockwise order
- // triangles : a list of triangles (result)
- //returns 1 on success, 0 on failure
- int Triangulate_MONO(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles);
-
- //triangulates a list of polygons by firstly partitioning them into monotone polygons
- //time complexity: O(n*log(n)), n is the number of vertices
- //space complexity: O(n)
- //params:
- // inpolys : a list of polygons to be triangulated (can contain holes)
- // vertices of all non-hole polys have to be in counter-clockwise order
- // vertices of all hole polys have to be in clockwise order
- // triangles : a list of triangles (result)
- //returns 1 on success, 0 on failure
- int Triangulate_MONO(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles);
-
- //creates a monotone partition of a list of polygons that can contain holes
- //time complexity: O(n*log(n)), n is the number of vertices
- //space complexity: O(n)
- //params:
- // inpolys : a list of polygons to be triangulated (can contain holes)
- // vertices of all non-hole polys have to be in counter-clockwise order
- // vertices of all hole polys have to be in clockwise order
- // monotonePolys : a list of monotone polygons (result)
- //returns 1 on success, 0 on failure
- int MonotonePartition(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *monotonePolys);
-
- //partitions a polygon into convex polygons by using Hertel-Mehlhorn algorithm
- //the algorithm gives at most four times the number of parts as the optimal algorithm
- //however, in practice it works much better than that and often gives optimal partition
- //uses triangulation obtained by ear clipping as intermediate result
- //time complexity O(n^2), n is the number of vertices
- //space complexity: O(n)
- //params:
- // poly : an input polygon to be partitioned
- // vertices have to be in counter-clockwise order
- // parts : resulting list of convex polygons
- //returns 1 on success, 0 on failure
- int ConvexPartition_HM(TriangulatorPoly *poly, List<TriangulatorPoly> *parts);
-
- //partitions a list of polygons into convex parts by using Hertel-Mehlhorn algorithm
- //the algorithm gives at most four times the number of parts as the optimal algorithm
- //however, in practice it works much better than that and often gives optimal partition
- //uses triangulation obtained by ear clipping as intermediate result
- //time complexity O(n^2), n is the number of vertices
- //space complexity: O(n)
- //params:
- // inpolys : an input list of polygons to be partitioned
- // vertices of all non-hole polys have to be in counter-clockwise order
- // vertices of all hole polys have to be in clockwise order
- // parts : resulting list of convex polygons
- //returns 1 on success, 0 on failure
- int ConvexPartition_HM(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *parts);
-
- //optimal convex partitioning (in terms of number of resulting convex polygons)
- //using the Keil-Snoeyink algorithm
- //M. Keil, J. Snoeyink, "On the time bound for convex decomposition of simple polygons", 1998
- //time complexity O(n^3), n is the number of vertices
- //space complexity: O(n^3)
- // poly : an input polygon to be partitioned
- // vertices have to be in counter-clockwise order
- // parts : resulting list of convex polygons
- //returns 1 on success, 0 on failure
- int ConvexPartition_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *parts);
-};
-
-
-#endif
diff --git a/core/math/vector3.h b/core/math/vector3.h
index a6bc20ccb..5f4390fbd 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -389,7 +389,8 @@ Vector3 Vector3::normalized() const {
}
bool Vector3::is_normalized() const {
- return Math::isequal_approx(length(), (real_t)1.0);
+ // use length_squared() instead of length() to avoid sqrt(), makes it more stringent.
+ return Math::is_equal_approx(length_squared(), 1.0);
}
Vector3 Vector3::inverse() const {
@@ -404,7 +405,7 @@ void Vector3::zero() {
// slide returns the component of the vector along the given plane, specified by its normal vector.
Vector3 Vector3::slide(const Vector3 &p_n) const {
-#ifdef DEBUG_ENABLED
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(p_n.is_normalized() == false, Vector3());
#endif
return *this - p_n * this->dot(p_n);
@@ -415,7 +416,7 @@ Vector3 Vector3::bounce(const Vector3 &p_n) const {
}
Vector3 Vector3::reflect(const Vector3 &p_n) const {
-#ifdef DEBUG_ENABLED
+#ifdef MATH_CHECKS
ERR_FAIL_COND_V(p_n.is_normalized() == false, Vector3());
#endif
return 2.0 * p_n * this->dot(p_n) - *this;
generated by cgit v1.2.3-70-g09d2 (git 2.52.0) at 2025-12-07 20:58:40 +0000