2014-02-10 01:10:30 +00:00
|
|
|
/*************************************************************************/
|
|
|
|
/* plane.cpp */
|
|
|
|
/*************************************************************************/
|
|
|
|
/* This file is part of: */
|
|
|
|
/* GODOT ENGINE */
|
2017-08-27 12:16:55 +00:00
|
|
|
/* https://godotengine.org */
|
2014-02-10 01:10:30 +00:00
|
|
|
/*************************************************************************/
|
2021-01-01 19:13:46 +00:00
|
|
|
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
|
|
|
|
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
|
2014-02-10 01:10:30 +00:00
|
|
|
/* */
|
|
|
|
/* 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. */
|
|
|
|
/*************************************************************************/
|
2018-01-04 23:50:27 +00:00
|
|
|
|
2014-02-10 01:10:30 +00:00
|
|
|
#include "plane.h"
|
|
|
|
|
2018-09-11 16:13:45 +00:00
|
|
|
#include "core/math/math_funcs.h"
|
2020-11-07 22:33:38 +00:00
|
|
|
#include "core/variant/variant.h"
|
2014-02-10 01:10:30 +00:00
|
|
|
|
2017-03-05 15:44:50 +00:00
|
|
|
void Plane::set_normal(const Vector3 &p_normal) {
|
|
|
|
normal = p_normal;
|
2014-02-10 01:10:30 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::normalize() {
|
|
|
|
real_t l = normal.length();
|
2017-03-05 15:44:50 +00:00
|
|
|
if (l == 0) {
|
|
|
|
*this = Plane(0, 0, 0, 0);
|
2014-02-10 01:10:30 +00:00
|
|
|
return;
|
|
|
|
}
|
2017-03-05 15:44:50 +00:00
|
|
|
normal /= l;
|
2020-05-10 14:47:11 +00:00
|
|
|
d /= l;
|
2014-02-10 01:10:30 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
Plane Plane::normalized() const {
|
|
|
|
Plane p = *this;
|
|
|
|
p.normalize();
|
|
|
|
return p;
|
|
|
|
}
|
|
|
|
|
|
|
|
Vector3 Plane::get_any_perpendicular_normal() const {
|
2017-03-05 15:44:50 +00:00
|
|
|
static const Vector3 p1 = Vector3(1, 0, 0);
|
|
|
|
static const Vector3 p2 = Vector3(0, 1, 0);
|
2014-02-10 01:10:30 +00:00
|
|
|
Vector3 p;
|
2016-03-08 23:00:52 +00:00
|
|
|
|
2020-05-14 14:41:43 +00:00
|
|
|
if (ABS(normal.dot(p1)) > 0.99) { // if too similar to p1
|
2017-03-05 15:44:50 +00:00
|
|
|
p = p2; // use p2
|
2020-05-14 14:41:43 +00:00
|
|
|
} else {
|
2017-03-05 15:44:50 +00:00
|
|
|
p = p1; // use p1
|
2020-05-14 14:41:43 +00:00
|
|
|
}
|
2016-03-08 23:00:52 +00:00
|
|
|
|
2017-03-05 15:44:50 +00:00
|
|
|
p -= normal * normal.dot(p);
|
2014-02-10 01:10:30 +00:00
|
|
|
p.normalize();
|
|
|
|
|
|
|
|
return p;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* intersections */
|
|
|
|
|
|
|
|
bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const {
|
2017-03-05 15:44:50 +00:00
|
|
|
const Plane &p_plane0 = *this;
|
|
|
|
Vector3 normal0 = p_plane0.normal;
|
|
|
|
Vector3 normal1 = p_plane1.normal;
|
|
|
|
Vector3 normal2 = p_plane2.normal;
|
2014-02-10 01:10:30 +00:00
|
|
|
|
2017-03-05 15:44:50 +00:00
|
|
|
real_t denom = vec3_cross(normal0, normal1).dot(normal2);
|
2014-02-10 01:10:30 +00:00
|
|
|
|
2020-05-14 14:41:43 +00:00
|
|
|
if (Math::is_zero_approx(denom)) {
|
2014-02-10 01:10:30 +00:00
|
|
|
return false;
|
2020-05-14 14:41:43 +00:00
|
|
|
}
|
2014-02-10 01:10:30 +00:00
|
|
|
|
2017-03-05 15:44:50 +00:00
|
|
|
if (r_result) {
|
2020-05-10 14:47:11 +00:00
|
|
|
*r_result = ((vec3_cross(normal1, normal2) * p_plane0.d) +
|
|
|
|
(vec3_cross(normal2, normal0) * p_plane1.d) +
|
|
|
|
(vec3_cross(normal0, normal1) * p_plane2.d)) /
|
2021-10-28 13:19:35 +00:00
|
|
|
denom;
|
2017-03-05 15:44:50 +00:00
|
|
|
}
|
2014-02-10 01:10:30 +00:00
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-08-12 11:04:30 +00:00
|
|
|
bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
|
2017-03-05 15:44:50 +00:00
|
|
|
Vector3 segment = p_dir;
|
|
|
|
real_t den = normal.dot(segment);
|
2014-02-10 01:10:30 +00:00
|
|
|
|
|
|
|
//printf("den is %i\n",den);
|
2019-04-25 17:19:14 +00:00
|
|
|
if (Math::is_zero_approx(den)) {
|
2014-02-10 01:10:30 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2020-05-10 14:47:11 +00:00
|
|
|
real_t dist = (normal.dot(p_from) - d) / den;
|
2014-02-10 01:10:30 +00:00
|
|
|
//printf("dist is %i\n",dist);
|
|
|
|
|
2019-01-13 10:06:34 +00:00
|
|
|
if (dist > CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist
|
2014-02-10 01:10:30 +00:00
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-01-13 10:06:34 +00:00
|
|
|
dist = -dist;
|
2014-02-10 01:10:30 +00:00
|
|
|
*p_intersection = p_from + segment * dist;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-08-12 11:04:30 +00:00
|
|
|
bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const {
|
2017-03-05 15:44:50 +00:00
|
|
|
Vector3 segment = p_begin - p_end;
|
|
|
|
real_t den = normal.dot(segment);
|
2014-02-10 01:10:30 +00:00
|
|
|
|
|
|
|
//printf("den is %i\n",den);
|
2019-04-25 17:19:14 +00:00
|
|
|
if (Math::is_zero_approx(den)) {
|
2014-02-10 01:10:30 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2020-05-10 14:47:11 +00:00
|
|
|
real_t dist = (normal.dot(p_begin) - d) / den;
|
2014-02-10 01:10:30 +00:00
|
|
|
//printf("dist is %i\n",dist);
|
|
|
|
|
2017-03-05 15:44:50 +00:00
|
|
|
if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
|
2014-02-10 01:10:30 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-01-13 10:06:34 +00:00
|
|
|
dist = -dist;
|
2014-02-10 01:10:30 +00:00
|
|
|
*p_intersection = p_begin + segment * dist;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-10-13 18:59:37 +00:00
|
|
|
Variant Plane::intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) const {
|
|
|
|
Vector3 inters;
|
|
|
|
if (intersect_3(p_plane1, p_plane2, &inters)) {
|
|
|
|
return inters;
|
|
|
|
} else {
|
|
|
|
return Variant();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Variant Plane::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const {
|
|
|
|
Vector3 inters;
|
|
|
|
if (intersects_ray(p_from, p_dir, &inters)) {
|
|
|
|
return inters;
|
|
|
|
} else {
|
|
|
|
return Variant();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Variant Plane::intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const {
|
|
|
|
Vector3 inters;
|
|
|
|
if (intersects_segment(p_begin, p_end, &inters)) {
|
|
|
|
return inters;
|
|
|
|
} else {
|
|
|
|
return Variant();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-02-10 01:10:30 +00:00
|
|
|
/* misc */
|
|
|
|
|
2020-05-01 12:34:23 +00:00
|
|
|
bool Plane::is_equal_approx_any_side(const Plane &p_plane) const {
|
|
|
|
return (normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d)) || (normal.is_equal_approx(-p_plane.normal) && Math::is_equal_approx(d, -p_plane.d));
|
|
|
|
}
|
|
|
|
|
2019-10-14 20:33:45 +00:00
|
|
|
bool Plane::is_equal_approx(const Plane &p_plane) const {
|
2020-05-10 14:47:11 +00:00
|
|
|
return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d);
|
2014-02-10 01:10:30 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
Plane::operator String() const {
|
2021-02-25 14:54:50 +00:00
|
|
|
return "[N: " + normal.operator String() + ", D: " + String::num_real(d, false) + "]";
|
2014-02-10 01:10:30 +00:00
|
|
|
}
|