diff options
Diffstat (limited to 'core/math/plane.cpp')
| -rw-r--r-- | core/math/plane.cpp | 166 |
1 files changed, 166 insertions, 0 deletions
diff --git a/core/math/plane.cpp b/core/math/plane.cpp new file mode 100644 index 0000000000..88c7be5f63 --- /dev/null +++ b/core/math/plane.cpp @@ -0,0 +1,166 @@ +/*************************************************************************/ +/* plane.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "plane.h" + +#include "math_funcs.h" + +#define _PLANE_EQ_DOT_EPSILON 0.999 +#define _PLANE_EQ_D_EPSILON 0.0001 + +void Plane::set_normal(const Vector3& p_normal) { + + normal=p_normal; +} + +void Plane::normalize() { + + real_t l = normal.length(); + if (l==0) { + *this=Plane(0,0,0,0); + return; + } + normal/=l; + d/=l; +} + +Plane Plane::normalized() const { + + Plane p = *this; + p.normalize(); + return p; +} + +Vector3 Plane::get_any_point() const { + + return get_normal()*d; +} + +Vector3 Plane::get_any_perpendicular_normal() const { + + static const Vector3 p1 = Vector3(1,0,0); + static const Vector3 p2 = Vector3(0,1,0); + Vector3 p; + + if (ABS(normal.dot(p1)) > 0.99) // if too similar to p1 + p=p2; // use p2 + else + p=p1; // use p1 + + p-=normal * normal.dot(p); + p.normalize(); + + return p; +} + +/* intersections */ + +bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const { + + const Plane &p_plane0=*this; + Vector3 normal0=p_plane0.normal; + Vector3 normal1=p_plane1.normal; + Vector3 normal2=p_plane2.normal; + + real_t denom=vec3_cross(normal0,normal1).dot(normal2); + + if (ABS(denom)<=CMP_EPSILON) + return false; + + if (r_result) { + *r_result = ( (vec3_cross(normal1, normal2) * p_plane0.d) + + (vec3_cross(normal2, normal0) * p_plane1.d) + + (vec3_cross(normal0, normal1) * p_plane2.d) )/denom; + } + + return true; +} + + +bool Plane::intersects_ray(Vector3 p_from, Vector3 p_dir, Vector3* p_intersection) const { + + Vector3 segment=p_dir; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_from ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist>CMP_EPSILON) { //this is a ray, before the emiting pos (p_from) doesnt exist + + return false; + } + + dist=-dist; + *p_intersection = p_from + segment * dist; + + return true; +} + +bool Plane::intersects_segment(Vector3 p_begin, Vector3 p_end, Vector3* p_intersection) const { + + Vector3 segment= p_begin - p_end; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_begin ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { + + return false; + } + + dist=-dist; + *p_intersection = p_begin + segment * dist; + + return true; +} + +/* misc */ + +bool Plane::is_almost_like(const Plane& p_plane) const { + + return (normal.dot( p_plane.normal ) > _PLANE_EQ_DOT_EPSILON && Math::absd(d-p_plane.d) < _PLANE_EQ_D_EPSILON); +} + + +Plane::operator String() const { + + return normal.operator String() + ", " + rtos(d); +} |