2203 lines
58 KiB
C++
2203 lines
58 KiB
C++
/*************************************************************************/
|
|
/* shape_3d_sw.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
|
|
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
|
|
/* */
|
|
/* 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 "shape_3d_sw.h"
|
|
|
|
#include "core/io/image.h"
|
|
#include "core/math/convex_hull.h"
|
|
#include "core/math/geometry_3d.h"
|
|
#include "core/templates/sort_array.h"
|
|
|
|
// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape.
|
|
|
|
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002
|
|
#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998
|
|
|
|
#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002
|
|
#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999
|
|
|
|
void Shape3DSW::configure(const AABB &p_aabb) {
|
|
aabb = p_aabb;
|
|
configured = true;
|
|
for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
|
|
ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key();
|
|
co->_shape_changed();
|
|
}
|
|
}
|
|
|
|
Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const {
|
|
Vector3 res;
|
|
int amnt;
|
|
FeatureType type;
|
|
get_supports(p_normal, 1, &res, amnt, type);
|
|
return res;
|
|
}
|
|
|
|
void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) {
|
|
Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
|
|
if (E) {
|
|
E->get()++;
|
|
} else {
|
|
owners[p_owner] = 1;
|
|
}
|
|
}
|
|
|
|
void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) {
|
|
Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
|
|
ERR_FAIL_COND(!E);
|
|
E->get()--;
|
|
if (E->get() == 0) {
|
|
owners.erase(E);
|
|
}
|
|
}
|
|
|
|
bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const {
|
|
return owners.has(p_owner);
|
|
}
|
|
|
|
const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const {
|
|
return owners;
|
|
}
|
|
|
|
Shape3DSW::~Shape3DSW() {
|
|
ERR_FAIL_COND(owners.size());
|
|
}
|
|
|
|
Plane WorldBoundaryShape3DSW::get_plane() const {
|
|
return plane;
|
|
}
|
|
|
|
void WorldBoundaryShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
// gibberish, a plane is infinity
|
|
r_min = -1e7;
|
|
r_max = 1e7;
|
|
}
|
|
|
|
Vector3 WorldBoundaryShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
return p_normal * 1e15;
|
|
}
|
|
|
|
bool WorldBoundaryShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
|
|
if (inters) {
|
|
r_normal = plane.normal;
|
|
}
|
|
return inters;
|
|
}
|
|
|
|
bool WorldBoundaryShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return plane.distance_to(p_point) < 0;
|
|
}
|
|
|
|
Vector3 WorldBoundaryShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
if (plane.is_point_over(p_point)) {
|
|
return plane.project(p_point);
|
|
} else {
|
|
return p_point;
|
|
}
|
|
}
|
|
|
|
Vector3 WorldBoundaryShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
return Vector3(); // not applicable.
|
|
}
|
|
|
|
void WorldBoundaryShape3DSW::_setup(const Plane &p_plane) {
|
|
plane = p_plane;
|
|
configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
|
|
}
|
|
|
|
void WorldBoundaryShape3DSW::set_data(const Variant &p_data) {
|
|
_setup(p_data);
|
|
}
|
|
|
|
Variant WorldBoundaryShape3DSW::get_data() const {
|
|
return plane;
|
|
}
|
|
|
|
WorldBoundaryShape3DSW::WorldBoundaryShape3DSW() {
|
|
}
|
|
|
|
//
|
|
|
|
real_t SeparationRayShape3DSW::get_length() const {
|
|
return length;
|
|
}
|
|
|
|
bool SeparationRayShape3DSW::get_slide_on_slope() const {
|
|
return slide_on_slope;
|
|
}
|
|
|
|
void SeparationRayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
// don't think this will be even used
|
|
r_min = 0;
|
|
r_max = 1;
|
|
}
|
|
|
|
Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
if (p_normal.z > 0) {
|
|
return Vector3(0, 0, length);
|
|
} else {
|
|
return Vector3(0, 0, 0);
|
|
}
|
|
}
|
|
|
|
void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
r_supports[0] = Vector3(0, 0, 0);
|
|
r_supports[1] = Vector3(0, 0, length);
|
|
} else if (p_normal.z > 0) {
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
*r_supports = Vector3(0, 0, length);
|
|
} else {
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
*r_supports = Vector3(0, 0, 0);
|
|
}
|
|
}
|
|
|
|
bool SeparationRayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
return false; //simply not possible
|
|
}
|
|
|
|
bool SeparationRayShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return false; //simply not possible
|
|
}
|
|
|
|
Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
Vector3 s[2] = {
|
|
Vector3(0, 0, 0),
|
|
Vector3(0, 0, length)
|
|
};
|
|
|
|
return Geometry3D::get_closest_point_to_segment(p_point, s);
|
|
}
|
|
|
|
Vector3 SeparationRayShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
return Vector3();
|
|
}
|
|
|
|
void SeparationRayShape3DSW::_setup(real_t p_length, bool p_slide_on_slope) {
|
|
length = p_length;
|
|
slide_on_slope = p_slide_on_slope;
|
|
configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
|
|
}
|
|
|
|
void SeparationRayShape3DSW::set_data(const Variant &p_data) {
|
|
Dictionary d = p_data;
|
|
_setup(d["length"], d["slide_on_slope"]);
|
|
}
|
|
|
|
Variant SeparationRayShape3DSW::get_data() const {
|
|
Dictionary d;
|
|
d["length"] = length;
|
|
d["slide_on_slope"] = slide_on_slope;
|
|
return d;
|
|
}
|
|
|
|
SeparationRayShape3DSW::SeparationRayShape3DSW() {}
|
|
|
|
/********** SPHERE *************/
|
|
|
|
real_t SphereShape3DSW::get_radius() const {
|
|
return radius;
|
|
}
|
|
|
|
void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
real_t d = p_normal.dot(p_transform.origin);
|
|
|
|
// figure out scale at point
|
|
Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
|
|
real_t scale = local_normal.length();
|
|
|
|
r_min = d - (radius)*scale;
|
|
r_max = d + (radius)*scale;
|
|
}
|
|
|
|
Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
return p_normal * radius;
|
|
}
|
|
|
|
void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
*r_supports = p_normal * radius;
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
}
|
|
|
|
bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
|
|
}
|
|
|
|
bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return p_point.length() < radius;
|
|
}
|
|
|
|
Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
Vector3 p = p_point;
|
|
real_t l = p.length();
|
|
if (l < radius) {
|
|
return p_point;
|
|
}
|
|
return (p / l) * radius;
|
|
}
|
|
|
|
Vector3 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
real_t s = 0.4 * p_mass * radius * radius;
|
|
return Vector3(s, s, s);
|
|
}
|
|
|
|
void SphereShape3DSW::_setup(real_t p_radius) {
|
|
radius = p_radius;
|
|
configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
|
|
}
|
|
|
|
void SphereShape3DSW::set_data(const Variant &p_data) {
|
|
_setup(p_data);
|
|
}
|
|
|
|
Variant SphereShape3DSW::get_data() const {
|
|
return radius;
|
|
}
|
|
|
|
SphereShape3DSW::SphereShape3DSW() {}
|
|
|
|
/********** BOX *************/
|
|
|
|
void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
// no matter the angle, the box is mirrored anyway
|
|
Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
|
|
|
|
real_t length = local_normal.abs().dot(half_extents);
|
|
real_t distance = p_normal.dot(p_transform.origin);
|
|
|
|
r_min = distance - length;
|
|
r_max = distance + length;
|
|
}
|
|
|
|
Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
Vector3 point(
|
|
(p_normal.x < 0) ? -half_extents.x : half_extents.x,
|
|
(p_normal.y < 0) ? -half_extents.y : half_extents.y,
|
|
(p_normal.z < 0) ? -half_extents.z : half_extents.z);
|
|
|
|
return point;
|
|
}
|
|
|
|
void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
static const int next[3] = { 1, 2, 0 };
|
|
static const int next2[3] = { 2, 0, 1 };
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
Vector3 axis;
|
|
axis[i] = 1.0;
|
|
real_t dot = p_normal.dot(axis);
|
|
if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
//Vector3 axis_b;
|
|
|
|
bool neg = dot < 0;
|
|
r_amount = 4;
|
|
r_type = FEATURE_FACE;
|
|
|
|
Vector3 point;
|
|
point[i] = half_extents[i];
|
|
|
|
int i_n = next[i];
|
|
int i_n2 = next2[i];
|
|
|
|
static const real_t sign[4][2] = {
|
|
{ -1.0, 1.0 },
|
|
{ 1.0, 1.0 },
|
|
{ 1.0, -1.0 },
|
|
{ -1.0, -1.0 },
|
|
};
|
|
|
|
for (int j = 0; j < 4; j++) {
|
|
point[i_n] = sign[j][0] * half_extents[i_n];
|
|
point[i_n2] = sign[j][1] * half_extents[i_n2];
|
|
r_supports[j] = neg ? -point : point;
|
|
}
|
|
|
|
if (neg) {
|
|
SWAP(r_supports[1], r_supports[2]);
|
|
SWAP(r_supports[0], r_supports[3]);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
r_amount = 0;
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
Vector3 axis;
|
|
axis[i] = 1.0;
|
|
|
|
if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
|
|
int i_n = next[i];
|
|
int i_n2 = next2[i];
|
|
|
|
Vector3 point = half_extents;
|
|
|
|
if (p_normal[i_n] < 0) {
|
|
point[i_n] = -point[i_n];
|
|
}
|
|
if (p_normal[i_n2] < 0) {
|
|
point[i_n2] = -point[i_n2];
|
|
}
|
|
|
|
r_supports[0] = point;
|
|
point[i] = -point[i];
|
|
r_supports[1] = point;
|
|
return;
|
|
}
|
|
}
|
|
/* USE POINT */
|
|
|
|
Vector3 point(
|
|
(p_normal.x < 0) ? -half_extents.x : half_extents.x,
|
|
(p_normal.y < 0) ? -half_extents.y : half_extents.y,
|
|
(p_normal.z < 0) ? -half_extents.z : half_extents.z);
|
|
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
r_supports[0] = point;
|
|
}
|
|
|
|
bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
AABB aabb(-half_extents, half_extents * 2.0);
|
|
|
|
return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
|
|
}
|
|
|
|
bool BoxShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z);
|
|
}
|
|
|
|
Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
int outside = 0;
|
|
Vector3 min_point;
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
if (Math::abs(p_point[i]) > half_extents[i]) {
|
|
outside++;
|
|
if (outside == 1) {
|
|
//use plane if only one side matches
|
|
Vector3 n;
|
|
n[i] = SGN(p_point[i]);
|
|
|
|
Plane p(n, half_extents[i]);
|
|
min_point = p.project(p_point);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!outside) {
|
|
return p_point; //it's inside, don't do anything else
|
|
}
|
|
|
|
if (outside == 1) { //if only above one plane, this plane clearly wins
|
|
return min_point;
|
|
}
|
|
|
|
//check segments
|
|
real_t min_distance = 1e20;
|
|
Vector3 closest_vertex = half_extents * p_point.sign();
|
|
Vector3 s[2] = {
|
|
closest_vertex,
|
|
closest_vertex
|
|
};
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
s[1] = closest_vertex;
|
|
s[1][i] = -s[1][i]; //edge
|
|
|
|
Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s);
|
|
|
|
real_t d = p_point.distance_to(closest_edge);
|
|
if (d < min_distance) {
|
|
min_point = closest_edge;
|
|
min_distance = d;
|
|
}
|
|
}
|
|
|
|
return min_point;
|
|
}
|
|
|
|
Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
real_t lx = half_extents.x;
|
|
real_t ly = half_extents.y;
|
|
real_t lz = half_extents.z;
|
|
|
|
return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly));
|
|
}
|
|
|
|
void BoxShape3DSW::_setup(const Vector3 &p_half_extents) {
|
|
half_extents = p_half_extents.abs();
|
|
|
|
configure(AABB(-half_extents, half_extents * 2));
|
|
}
|
|
|
|
void BoxShape3DSW::set_data(const Variant &p_data) {
|
|
_setup(p_data);
|
|
}
|
|
|
|
Variant BoxShape3DSW::get_data() const {
|
|
return half_extents;
|
|
}
|
|
|
|
BoxShape3DSW::BoxShape3DSW() {}
|
|
|
|
/********** CAPSULE *************/
|
|
|
|
void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
|
|
real_t h = height * 0.5 - radius;
|
|
|
|
n *= radius;
|
|
n.y += (n.y > 0) ? h : -h;
|
|
|
|
r_max = p_normal.dot(p_transform.xform(n));
|
|
r_min = p_normal.dot(p_transform.xform(-n));
|
|
}
|
|
|
|
Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
Vector3 n = p_normal;
|
|
|
|
real_t h = height * 0.5 - radius;
|
|
|
|
n *= radius;
|
|
n.y += (n.y > 0) ? h : -h;
|
|
return n;
|
|
}
|
|
|
|
void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
Vector3 n = p_normal;
|
|
|
|
real_t d = n.y;
|
|
|
|
if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
// make it flat
|
|
n.y = 0.0;
|
|
n.normalize();
|
|
n *= radius;
|
|
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
r_supports[0] = n;
|
|
r_supports[0].y += height * 0.5 - radius;
|
|
r_supports[1] = n;
|
|
r_supports[1].y -= height * 0.5 - radius;
|
|
|
|
} else {
|
|
real_t h = height * 0.5 - radius;
|
|
|
|
n *= radius;
|
|
n.y += (d > 0) ? h : -h;
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
*r_supports = n;
|
|
}
|
|
}
|
|
|
|
bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
Vector3 norm = (p_end - p_begin).normalized();
|
|
real_t min_d = 1e20;
|
|
|
|
Vector3 res, n;
|
|
bool collision = false;
|
|
|
|
Vector3 auxres, auxn;
|
|
bool collided;
|
|
|
|
// test against cylinder and spheres :-|
|
|
|
|
collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1);
|
|
|
|
if (collided) {
|
|
real_t d = norm.dot(auxres);
|
|
if (d < min_d) {
|
|
min_d = d;
|
|
res = auxres;
|
|
n = auxn;
|
|
collision = true;
|
|
}
|
|
}
|
|
|
|
collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn);
|
|
|
|
if (collided) {
|
|
real_t d = norm.dot(auxres);
|
|
if (d < min_d) {
|
|
min_d = d;
|
|
res = auxres;
|
|
n = auxn;
|
|
collision = true;
|
|
}
|
|
}
|
|
|
|
collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn);
|
|
|
|
if (collided) {
|
|
real_t d = norm.dot(auxres);
|
|
|
|
if (d < min_d) {
|
|
min_d = d;
|
|
res = auxres;
|
|
n = auxn;
|
|
collision = true;
|
|
}
|
|
}
|
|
|
|
if (collision) {
|
|
r_result = res;
|
|
r_normal = n;
|
|
}
|
|
return collision;
|
|
}
|
|
|
|
bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
if (Math::abs(p_point.y) < height * 0.5 - radius) {
|
|
return Vector3(p_point.x, 0, p_point.z).length() < radius;
|
|
} else {
|
|
Vector3 p = p_point;
|
|
p.y = Math::abs(p.y) - height * 0.5 + radius;
|
|
return p.length() < radius;
|
|
}
|
|
}
|
|
|
|
Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
Vector3 s[2] = {
|
|
Vector3(0, -height * 0.5 + radius, 0),
|
|
Vector3(0, height * 0.5 - radius, 0),
|
|
};
|
|
|
|
Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
|
|
|
|
if (p.distance_to(p_point) < radius) {
|
|
return p_point;
|
|
}
|
|
|
|
return p + (p_point - p).normalized() * radius;
|
|
}
|
|
|
|
Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
// use bad AABB approximation
|
|
Vector3 extents = get_aabb().size * 0.5;
|
|
|
|
return Vector3(
|
|
(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
|
|
}
|
|
|
|
void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) {
|
|
height = p_height;
|
|
radius = p_radius;
|
|
configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2)));
|
|
}
|
|
|
|
void CapsuleShape3DSW::set_data(const Variant &p_data) {
|
|
Dictionary d = p_data;
|
|
ERR_FAIL_COND(!d.has("radius"));
|
|
ERR_FAIL_COND(!d.has("height"));
|
|
_setup(d["height"], d["radius"]);
|
|
}
|
|
|
|
Variant CapsuleShape3DSW::get_data() const {
|
|
Dictionary d;
|
|
d["radius"] = radius;
|
|
d["height"] = height;
|
|
return d;
|
|
}
|
|
|
|
CapsuleShape3DSW::CapsuleShape3DSW() {}
|
|
|
|
/********** CYLINDER *************/
|
|
|
|
void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized();
|
|
real_t axis_dot = cylinder_axis.dot(p_normal);
|
|
|
|
Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
|
|
real_t scale = local_normal.length();
|
|
real_t scaled_radius = radius * scale;
|
|
real_t scaled_height = height * scale;
|
|
|
|
real_t length;
|
|
if (Math::abs(axis_dot) > 1.0) {
|
|
length = scaled_height * 0.5;
|
|
} else {
|
|
length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot);
|
|
}
|
|
|
|
real_t distance = p_normal.dot(p_transform.origin);
|
|
|
|
r_min = distance - length;
|
|
r_max = distance + length;
|
|
}
|
|
|
|
Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
Vector3 n = p_normal;
|
|
real_t h = (n.y > 0) ? height : -height;
|
|
real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
|
|
if (Math::is_zero_approx(s)) {
|
|
n.x = radius;
|
|
n.y = h * 0.5;
|
|
n.z = 0.0;
|
|
} else {
|
|
real_t d = radius / s;
|
|
n.x = n.x * d;
|
|
n.y = h * 0.5;
|
|
n.z = n.z * d;
|
|
}
|
|
|
|
return n;
|
|
}
|
|
|
|
void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
real_t d = p_normal.y;
|
|
if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
real_t h = (d > 0) ? height : -height;
|
|
|
|
Vector3 n = p_normal;
|
|
n.x = 0.0;
|
|
n.z = 0.0;
|
|
n.y = h * 0.5;
|
|
|
|
r_amount = 3;
|
|
r_type = FEATURE_CIRCLE;
|
|
r_supports[0] = n;
|
|
r_supports[1] = n;
|
|
r_supports[1].x += radius;
|
|
r_supports[2] = n;
|
|
r_supports[2].z += radius;
|
|
} else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
// make it flat
|
|
Vector3 n = p_normal;
|
|
n.y = 0.0;
|
|
n.normalize();
|
|
n *= radius;
|
|
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
r_supports[0] = n;
|
|
r_supports[0].y += height * 0.5;
|
|
r_supports[1] = n;
|
|
r_supports[1].y -= height * 0.5;
|
|
} else {
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
r_supports[0] = get_support(p_normal);
|
|
return;
|
|
|
|
Vector3 n = p_normal;
|
|
real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius);
|
|
if (Math::abs(h) > 1.0) {
|
|
// Top or bottom surface.
|
|
n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5;
|
|
} else {
|
|
// Lateral surface.
|
|
n.y = height * 0.5 * h;
|
|
}
|
|
|
|
real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
|
|
if (Math::is_zero_approx(s)) {
|
|
n.x = 0.0;
|
|
n.z = 0.0;
|
|
} else {
|
|
real_t scaled_radius = radius / s;
|
|
n.x = n.x * scaled_radius;
|
|
n.z = n.z * scaled_radius;
|
|
}
|
|
|
|
r_supports[0] = n;
|
|
}
|
|
}
|
|
|
|
bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1);
|
|
}
|
|
|
|
bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
if (Math::abs(p_point.y) < height * 0.5) {
|
|
return Vector3(p_point.x, 0, p_point.z).length() < radius;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
if (Math::absf(p_point.y) > height * 0.5) {
|
|
// Project point to top disk.
|
|
real_t dir = p_point.y > 0.0 ? 1.0 : -1.0;
|
|
Vector3 circle_pos(0.0, dir * height * 0.5, 0.0);
|
|
Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0));
|
|
Vector3 proj_point = circle_plane.project(p_point);
|
|
|
|
// Clip position.
|
|
Vector3 delta_point_1 = proj_point - circle_pos;
|
|
real_t dist_point_1 = delta_point_1.length_squared();
|
|
if (!Math::is_zero_approx(dist_point_1)) {
|
|
dist_point_1 = Math::sqrt(dist_point_1);
|
|
proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1;
|
|
}
|
|
|
|
return proj_point;
|
|
} else {
|
|
Vector3 s[2] = {
|
|
Vector3(0, -height * 0.5, 0),
|
|
Vector3(0, height * 0.5, 0),
|
|
};
|
|
|
|
Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
|
|
|
|
if (p.distance_to(p_point) < radius) {
|
|
return p_point;
|
|
}
|
|
|
|
return p + (p_point - p).normalized() * radius;
|
|
}
|
|
}
|
|
|
|
Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
// use bad AABB approximation
|
|
Vector3 extents = get_aabb().size * 0.5;
|
|
|
|
return Vector3(
|
|
(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
|
|
}
|
|
|
|
void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) {
|
|
height = p_height;
|
|
radius = p_radius;
|
|
configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0)));
|
|
}
|
|
|
|
void CylinderShape3DSW::set_data(const Variant &p_data) {
|
|
Dictionary d = p_data;
|
|
ERR_FAIL_COND(!d.has("radius"));
|
|
ERR_FAIL_COND(!d.has("height"));
|
|
_setup(d["height"], d["radius"]);
|
|
}
|
|
|
|
Variant CylinderShape3DSW::get_data() const {
|
|
Dictionary d;
|
|
d["radius"] = radius;
|
|
d["height"] = height;
|
|
return d;
|
|
}
|
|
|
|
CylinderShape3DSW::CylinderShape3DSW() {}
|
|
|
|
/********** CONVEX POLYGON *************/
|
|
|
|
void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
int vertex_count = mesh.vertices.size();
|
|
if (vertex_count == 0) {
|
|
return;
|
|
}
|
|
|
|
const Vector3 *vrts = &mesh.vertices[0];
|
|
|
|
for (int i = 0; i < vertex_count; i++) {
|
|
real_t d = p_normal.dot(p_transform.xform(vrts[i]));
|
|
|
|
if (i == 0 || d > r_max) {
|
|
r_max = d;
|
|
}
|
|
if (i == 0 || d < r_min) {
|
|
r_min = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
Vector3 n = p_normal;
|
|
|
|
int vert_support_idx = -1;
|
|
real_t support_max = 0;
|
|
|
|
int vertex_count = mesh.vertices.size();
|
|
if (vertex_count == 0) {
|
|
return Vector3();
|
|
}
|
|
|
|
const Vector3 *vrts = &mesh.vertices[0];
|
|
|
|
for (int i = 0; i < vertex_count; i++) {
|
|
real_t d = n.dot(vrts[i]);
|
|
|
|
if (i == 0 || d > support_max) {
|
|
support_max = d;
|
|
vert_support_idx = i;
|
|
}
|
|
}
|
|
|
|
return vrts[vert_support_idx];
|
|
}
|
|
|
|
void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
|
|
int fc = mesh.faces.size();
|
|
|
|
const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
|
|
int ec = mesh.edges.size();
|
|
|
|
const Vector3 *vertices = mesh.vertices.ptr();
|
|
int vc = mesh.vertices.size();
|
|
|
|
r_amount = 0;
|
|
ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices.");
|
|
|
|
//find vertex first
|
|
real_t max = 0;
|
|
int vtx = 0;
|
|
|
|
for (int i = 0; i < vc; i++) {
|
|
real_t d = p_normal.dot(vertices[i]);
|
|
|
|
if (i == 0 || d > max) {
|
|
max = d;
|
|
vtx = i;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < fc; i++) {
|
|
if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
int ic = faces[i].indices.size();
|
|
const int *ind = faces[i].indices.ptr();
|
|
|
|
bool valid = false;
|
|
for (int j = 0; j < ic; j++) {
|
|
if (ind[j] == vtx) {
|
|
valid = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!valid) {
|
|
continue;
|
|
}
|
|
|
|
int m = MIN(p_max, ic);
|
|
for (int j = 0; j < m; j++) {
|
|
r_supports[j] = vertices[ind[j]];
|
|
}
|
|
r_amount = m;
|
|
r_type = FEATURE_FACE;
|
|
return;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < ec; i++) {
|
|
real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
|
|
dot = ABS(dot);
|
|
if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) {
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
r_supports[0] = vertices[edges[i].a];
|
|
r_supports[1] = vertices[edges[i].b];
|
|
return;
|
|
}
|
|
}
|
|
|
|
r_supports[0] = vertices[vtx];
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
}
|
|
|
|
bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
|
|
int fc = mesh.faces.size();
|
|
|
|
const Vector3 *vertices = mesh.vertices.ptr();
|
|
|
|
Vector3 n = p_end - p_begin;
|
|
real_t min = 1e20;
|
|
bool col = false;
|
|
|
|
for (int i = 0; i < fc; i++) {
|
|
if (faces[i].plane.normal.dot(n) > 0) {
|
|
continue; //opposing face
|
|
}
|
|
|
|
int ic = faces[i].indices.size();
|
|
const int *ind = faces[i].indices.ptr();
|
|
|
|
for (int j = 1; j < ic - 1; j++) {
|
|
Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]);
|
|
Vector3 result;
|
|
if (f.intersects_segment(p_begin, p_end, &result)) {
|
|
real_t d = n.dot(result);
|
|
if (d < min) {
|
|
min = d;
|
|
r_result = result;
|
|
r_normal = faces[i].plane.normal;
|
|
col = true;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return col;
|
|
}
|
|
|
|
bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
|
|
int fc = mesh.faces.size();
|
|
|
|
for (int i = 0; i < fc; i++) {
|
|
if (faces[i].plane.distance_to(p_point) >= 0) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
|
|
int fc = mesh.faces.size();
|
|
const Vector3 *vertices = mesh.vertices.ptr();
|
|
|
|
bool all_inside = true;
|
|
for (int i = 0; i < fc; i++) {
|
|
if (!faces[i].plane.is_point_over(p_point)) {
|
|
continue;
|
|
}
|
|
|
|
all_inside = false;
|
|
bool is_inside = true;
|
|
int ic = faces[i].indices.size();
|
|
const int *indices = faces[i].indices.ptr();
|
|
|
|
for (int j = 0; j < ic; j++) {
|
|
Vector3 a = vertices[indices[j]];
|
|
Vector3 b = vertices[indices[(j + 1) % ic]];
|
|
Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
|
|
if (Plane(a, n).is_point_over(p_point)) {
|
|
is_inside = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (is_inside) {
|
|
return faces[i].plane.project(p_point);
|
|
}
|
|
}
|
|
|
|
if (all_inside) {
|
|
return p_point;
|
|
}
|
|
|
|
real_t min_distance = 1e20;
|
|
Vector3 min_point;
|
|
|
|
//check edges
|
|
const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
|
|
int ec = mesh.edges.size();
|
|
for (int i = 0; i < ec; i++) {
|
|
Vector3 s[2] = {
|
|
vertices[edges[i].a],
|
|
vertices[edges[i].b]
|
|
};
|
|
|
|
Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s);
|
|
real_t d = closest.distance_to(p_point);
|
|
if (d < min_distance) {
|
|
min_distance = d;
|
|
min_point = closest;
|
|
}
|
|
}
|
|
|
|
return min_point;
|
|
}
|
|
|
|
Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
// use bad AABB approximation
|
|
Vector3 extents = get_aabb().size * 0.5;
|
|
|
|
return Vector3(
|
|
(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
|
|
}
|
|
|
|
void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
|
|
Error err = ConvexHullComputer::convex_hull(p_vertices, mesh);
|
|
if (err != OK) {
|
|
ERR_PRINT("Failed to build convex hull");
|
|
}
|
|
|
|
AABB _aabb;
|
|
|
|
for (int i = 0; i < mesh.vertices.size(); i++) {
|
|
if (i == 0) {
|
|
_aabb.position = mesh.vertices[i];
|
|
} else {
|
|
_aabb.expand_to(mesh.vertices[i]);
|
|
}
|
|
}
|
|
|
|
configure(_aabb);
|
|
}
|
|
|
|
void ConvexPolygonShape3DSW::set_data(const Variant &p_data) {
|
|
_setup(p_data);
|
|
}
|
|
|
|
Variant ConvexPolygonShape3DSW::get_data() const {
|
|
return mesh.vertices;
|
|
}
|
|
|
|
ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() {
|
|
}
|
|
|
|
/********** FACE POLYGON *************/
|
|
|
|
void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
for (int i = 0; i < 3; i++) {
|
|
Vector3 v = p_transform.xform(vertex[i]);
|
|
real_t d = p_normal.dot(v);
|
|
|
|
if (i == 0 || d > r_max) {
|
|
r_max = d;
|
|
}
|
|
|
|
if (i == 0 || d < r_min) {
|
|
r_min = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
int vert_support_idx = -1;
|
|
real_t support_max = 0;
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
real_t d = p_normal.dot(vertex[i]);
|
|
|
|
if (i == 0 || d > support_max) {
|
|
support_max = d;
|
|
vert_support_idx = i;
|
|
}
|
|
}
|
|
|
|
return vertex[vert_support_idx];
|
|
}
|
|
|
|
void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
|
Vector3 n = p_normal;
|
|
|
|
/** TEST FACE AS SUPPORT **/
|
|
if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
r_amount = 3;
|
|
r_type = FEATURE_FACE;
|
|
for (int i = 0; i < 3; i++) {
|
|
r_supports[i] = vertex[i];
|
|
}
|
|
return;
|
|
}
|
|
|
|
/** FIND SUPPORT VERTEX **/
|
|
|
|
int vert_support_idx = -1;
|
|
real_t support_max = 0;
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
real_t d = n.dot(vertex[i]);
|
|
|
|
if (i == 0 || d > support_max) {
|
|
support_max = d;
|
|
vert_support_idx = i;
|
|
}
|
|
}
|
|
|
|
/** TEST EDGES AS SUPPORT **/
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
int nx = (i + 1) % 3;
|
|
if (i != vert_support_idx && nx != vert_support_idx) {
|
|
continue;
|
|
}
|
|
|
|
// check if edge is valid as a support
|
|
real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n);
|
|
dot = ABS(dot);
|
|
if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
|
|
r_amount = 2;
|
|
r_type = FEATURE_EDGE;
|
|
r_supports[0] = vertex[i];
|
|
r_supports[1] = vertex[nx];
|
|
return;
|
|
}
|
|
}
|
|
|
|
r_amount = 1;
|
|
r_type = FEATURE_POINT;
|
|
r_supports[0] = vertex[vert_support_idx];
|
|
}
|
|
|
|
bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
|
|
if (c) {
|
|
r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
|
|
if (r_normal.dot(p_end - p_begin) > 0) {
|
|
if (backface_collision) {
|
|
r_normal = -r_normal;
|
|
} else {
|
|
c = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
return c;
|
|
}
|
|
|
|
bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return false; //face is flat
|
|
}
|
|
|
|
Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
|
|
}
|
|
|
|
Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
|
|
}
|
|
|
|
FaceShape3DSW::FaceShape3DSW() {
|
|
configure(AABB());
|
|
}
|
|
|
|
Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const {
|
|
Vector<Vector3> rfaces;
|
|
rfaces.resize(faces.size() * 3);
|
|
|
|
for (int i = 0; i < faces.size(); i++) {
|
|
Face f = faces.get(i);
|
|
|
|
for (int j = 0; j < 3; j++) {
|
|
rfaces.set(i * 3 + j, vertices.get(f.indices[j]));
|
|
}
|
|
}
|
|
|
|
return rfaces;
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
int count = vertices.size();
|
|
if (count == 0) {
|
|
r_min = 0;
|
|
r_max = 0;
|
|
return;
|
|
}
|
|
const Vector3 *vptr = vertices.ptr();
|
|
|
|
for (int i = 0; i < count; i++) {
|
|
real_t d = p_normal.dot(p_transform.xform(vptr[i]));
|
|
|
|
if (i == 0 || d > r_max) {
|
|
r_max = d;
|
|
}
|
|
if (i == 0 || d < r_min) {
|
|
r_min = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
int count = vertices.size();
|
|
if (count == 0) {
|
|
return Vector3();
|
|
}
|
|
|
|
const Vector3 *vptr = vertices.ptr();
|
|
|
|
Vector3 n = p_normal;
|
|
|
|
int vert_support_idx = -1;
|
|
real_t support_max = 0;
|
|
|
|
for (int i = 0; i < count; i++) {
|
|
real_t d = n.dot(vptr[i]);
|
|
|
|
if (i == 0 || d > support_max) {
|
|
support_max = d;
|
|
vert_support_idx = i;
|
|
}
|
|
}
|
|
|
|
return vptr[vert_support_idx];
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
|
|
const BVH *bvh = &p_params->bvh[p_idx];
|
|
|
|
/*
|
|
if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
|
|
return; //test against whole AABB, which isn't very costly
|
|
*/
|
|
|
|
//printf("addr: %p\n",bvh);
|
|
if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
|
|
return;
|
|
}
|
|
|
|
if (bvh->face_index >= 0) {
|
|
const Face *f = &p_params->faces[bvh->face_index];
|
|
FaceShape3DSW *face = p_params->face;
|
|
face->normal = f->normal;
|
|
face->vertex[0] = p_params->vertices[f->indices[0]];
|
|
face->vertex[1] = p_params->vertices[f->indices[1]];
|
|
face->vertex[2] = p_params->vertices[f->indices[2]];
|
|
|
|
Vector3 res;
|
|
Vector3 normal;
|
|
if (face->intersect_segment(p_params->from, p_params->to, res, normal)) {
|
|
real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
|
|
if ((d > 0) && (d < p_params->min_d)) {
|
|
p_params->min_d = d;
|
|
p_params->result = res;
|
|
p_params->normal = normal;
|
|
p_params->collisions++;
|
|
}
|
|
}
|
|
} else {
|
|
if (bvh->left >= 0) {
|
|
_cull_segment(bvh->left, p_params);
|
|
}
|
|
if (bvh->right >= 0) {
|
|
_cull_segment(bvh->right, p_params);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
|
if (faces.size() == 0) {
|
|
return false;
|
|
}
|
|
|
|
// unlock data
|
|
const Face *fr = faces.ptr();
|
|
const Vector3 *vr = vertices.ptr();
|
|
const BVH *br = bvh.ptr();
|
|
|
|
FaceShape3DSW face;
|
|
face.backface_collision = backface_collision;
|
|
|
|
_SegmentCullParams params;
|
|
params.from = p_begin;
|
|
params.to = p_end;
|
|
params.dir = (p_end - p_begin).normalized();
|
|
|
|
params.faces = fr;
|
|
params.vertices = vr;
|
|
params.bvh = br;
|
|
|
|
params.face = &face;
|
|
|
|
// cull
|
|
_cull_segment(0, ¶ms);
|
|
|
|
if (params.collisions > 0) {
|
|
r_result = params.result;
|
|
r_normal = params.normal;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return false; //face is flat
|
|
}
|
|
|
|
Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
return Vector3();
|
|
}
|
|
|
|
bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
|
|
const BVH *bvh = &p_params->bvh[p_idx];
|
|
|
|
if (!p_params->aabb.intersects(bvh->aabb)) {
|
|
return false;
|
|
}
|
|
|
|
if (bvh->face_index >= 0) {
|
|
const Face *f = &p_params->faces[bvh->face_index];
|
|
FaceShape3DSW *face = p_params->face;
|
|
face->normal = f->normal;
|
|
face->vertex[0] = p_params->vertices[f->indices[0]];
|
|
face->vertex[1] = p_params->vertices[f->indices[1]];
|
|
face->vertex[2] = p_params->vertices[f->indices[2]];
|
|
if (p_params->callback(p_params->userdata, face)) {
|
|
return true;
|
|
}
|
|
} else {
|
|
if (bvh->left >= 0) {
|
|
if (_cull(bvh->left, p_params)) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
if (bvh->right >= 0) {
|
|
if (_cull(bvh->right, p_params)) {
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
|
|
// make matrix local to concave
|
|
if (faces.size() == 0) {
|
|
return;
|
|
}
|
|
|
|
AABB local_aabb = p_local_aabb;
|
|
|
|
// unlock data
|
|
const Face *fr = faces.ptr();
|
|
const Vector3 *vr = vertices.ptr();
|
|
const BVH *br = bvh.ptr();
|
|
|
|
FaceShape3DSW face; // use this to send in the callback
|
|
face.backface_collision = backface_collision;
|
|
|
|
_CullParams params;
|
|
params.aabb = local_aabb;
|
|
params.face = &face;
|
|
params.faces = fr;
|
|
params.vertices = vr;
|
|
params.bvh = br;
|
|
params.callback = p_callback;
|
|
params.userdata = p_userdata;
|
|
|
|
// cull
|
|
_cull(0, ¶ms);
|
|
}
|
|
|
|
Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
// use bad AABB approximation
|
|
Vector3 extents = get_aabb().size * 0.5;
|
|
|
|
return Vector3(
|
|
(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
|
|
}
|
|
|
|
struct _VolumeSW_BVH_Element {
|
|
AABB aabb;
|
|
Vector3 center;
|
|
int face_index;
|
|
};
|
|
|
|
struct _VolumeSW_BVH_CompareX {
|
|
_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
|
|
return a.center.x < b.center.x;
|
|
}
|
|
};
|
|
|
|
struct _VolumeSW_BVH_CompareY {
|
|
_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
|
|
return a.center.y < b.center.y;
|
|
}
|
|
};
|
|
|
|
struct _VolumeSW_BVH_CompareZ {
|
|
_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
|
|
return a.center.z < b.center.z;
|
|
}
|
|
};
|
|
|
|
struct _VolumeSW_BVH {
|
|
AABB aabb;
|
|
_VolumeSW_BVH *left;
|
|
_VolumeSW_BVH *right;
|
|
|
|
int face_index;
|
|
};
|
|
|
|
_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
|
|
_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
|
|
|
|
if (p_size == 1) {
|
|
//leaf
|
|
bvh->aabb = p_elements[0].aabb;
|
|
bvh->left = nullptr;
|
|
bvh->right = nullptr;
|
|
bvh->face_index = p_elements->face_index;
|
|
count++;
|
|
return bvh;
|
|
} else {
|
|
bvh->face_index = -1;
|
|
}
|
|
|
|
AABB aabb;
|
|
for (int i = 0; i < p_size; i++) {
|
|
if (i == 0) {
|
|
aabb = p_elements[i].aabb;
|
|
} else {
|
|
aabb.merge_with(p_elements[i].aabb);
|
|
}
|
|
}
|
|
bvh->aabb = aabb;
|
|
switch (aabb.get_longest_axis_index()) {
|
|
case 0: {
|
|
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
|
|
sort_x.sort(p_elements, p_size);
|
|
|
|
} break;
|
|
case 1: {
|
|
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
|
|
sort_y.sort(p_elements, p_size);
|
|
} break;
|
|
case 2: {
|
|
SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
|
|
sort_z.sort(p_elements, p_size);
|
|
} break;
|
|
}
|
|
|
|
int split = p_size / 2;
|
|
bvh->left = _volume_sw_build_bvh(p_elements, split, count);
|
|
bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
|
|
|
|
//printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
|
|
count++;
|
|
return bvh;
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
|
|
int idx = p_idx;
|
|
|
|
p_bvh_array[idx].aabb = p_bvh_tree->aabb;
|
|
p_bvh_array[idx].face_index = p_bvh_tree->face_index;
|
|
//printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
|
|
|
|
if (p_bvh_tree->left) {
|
|
p_bvh_array[idx].left = ++p_idx;
|
|
_fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx);
|
|
|
|
} else {
|
|
p_bvh_array[p_idx].left = -1;
|
|
}
|
|
|
|
if (p_bvh_tree->right) {
|
|
p_bvh_array[idx].right = ++p_idx;
|
|
_fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx);
|
|
|
|
} else {
|
|
p_bvh_array[p_idx].right = -1;
|
|
}
|
|
|
|
memdelete(p_bvh_tree);
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) {
|
|
int src_face_count = p_faces.size();
|
|
if (src_face_count == 0) {
|
|
configure(AABB());
|
|
return;
|
|
}
|
|
ERR_FAIL_COND(src_face_count % 3);
|
|
src_face_count /= 3;
|
|
|
|
const Vector3 *facesr = p_faces.ptr();
|
|
|
|
Vector<_VolumeSW_BVH_Element> bvh_array;
|
|
bvh_array.resize(src_face_count);
|
|
|
|
_VolumeSW_BVH_Element *bvh_arrayw = bvh_array.ptrw();
|
|
|
|
faces.resize(src_face_count);
|
|
Face *facesw = faces.ptrw();
|
|
|
|
vertices.resize(src_face_count * 3);
|
|
|
|
Vector3 *verticesw = vertices.ptrw();
|
|
|
|
AABB _aabb;
|
|
|
|
for (int i = 0; i < src_face_count; i++) {
|
|
Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
|
|
|
|
bvh_arrayw[i].aabb = face.get_aabb();
|
|
bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center();
|
|
bvh_arrayw[i].face_index = i;
|
|
facesw[i].indices[0] = i * 3 + 0;
|
|
facesw[i].indices[1] = i * 3 + 1;
|
|
facesw[i].indices[2] = i * 3 + 2;
|
|
facesw[i].normal = face.get_plane().normal;
|
|
verticesw[i * 3 + 0] = face.vertex[0];
|
|
verticesw[i * 3 + 1] = face.vertex[1];
|
|
verticesw[i * 3 + 2] = face.vertex[2];
|
|
if (i == 0) {
|
|
_aabb = bvh_arrayw[i].aabb;
|
|
} else {
|
|
_aabb.merge_with(bvh_arrayw[i].aabb);
|
|
}
|
|
}
|
|
|
|
int count = 0;
|
|
_VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
|
|
|
|
bvh.resize(count + 1);
|
|
|
|
BVH *bvh_arrayw2 = bvh.ptrw();
|
|
|
|
int idx = 0;
|
|
_fill_bvh(bvh_tree, bvh_arrayw2, idx);
|
|
|
|
backface_collision = p_backface_collision;
|
|
|
|
configure(_aabb); // this type of shape has no margin
|
|
}
|
|
|
|
void ConcavePolygonShape3DSW::set_data(const Variant &p_data) {
|
|
Dictionary d = p_data;
|
|
ERR_FAIL_COND(!d.has("faces"));
|
|
|
|
_setup(d["faces"], d["backface_collision"]);
|
|
}
|
|
|
|
Variant ConcavePolygonShape3DSW::get_data() const {
|
|
Dictionary d;
|
|
d["faces"] = get_faces();
|
|
d["backface_collision"] = backface_collision;
|
|
|
|
return d;
|
|
}
|
|
|
|
ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
|
|
}
|
|
|
|
/* HEIGHT MAP SHAPE */
|
|
|
|
Vector<real_t> HeightMapShape3DSW::get_heights() const {
|
|
return heights;
|
|
}
|
|
|
|
int HeightMapShape3DSW::get_width() const {
|
|
return width;
|
|
}
|
|
|
|
int HeightMapShape3DSW::get_depth() const {
|
|
return depth;
|
|
}
|
|
|
|
void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
|
|
//not very useful, but not very used either
|
|
p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
|
|
}
|
|
|
|
Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const {
|
|
//not very useful, but not very used either
|
|
return get_aabb().get_support(p_normal);
|
|
}
|
|
|
|
struct _HeightmapSegmentCullParams {
|
|
Vector3 from;
|
|
Vector3 to;
|
|
Vector3 dir;
|
|
|
|
Vector3 result;
|
|
Vector3 normal;
|
|
|
|
const HeightMapShape3DSW *heightmap = nullptr;
|
|
FaceShape3DSW *face = nullptr;
|
|
};
|
|
|
|
struct _HeightmapGridCullState {
|
|
real_t length = 0.0;
|
|
real_t length_flat = 0.0;
|
|
|
|
real_t dist = 0.0;
|
|
real_t prev_dist = 0.0;
|
|
|
|
int x = 0;
|
|
int z = 0;
|
|
};
|
|
|
|
_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) {
|
|
Vector3 res;
|
|
Vector3 normal;
|
|
if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) {
|
|
p_params.result = res;
|
|
p_params.normal = normal;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
|
|
// First triangle.
|
|
p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]);
|
|
p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]);
|
|
p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]);
|
|
p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
|
|
if (_heightmap_face_cull_segment(p_params)) {
|
|
return true;
|
|
}
|
|
|
|
// Second triangle.
|
|
p_params.face->vertex[0] = p_params.face->vertex[1];
|
|
p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]);
|
|
p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
|
|
if (_heightmap_face_cull_segment(p_params)) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
|
|
const HeightMapShape3DSW::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z);
|
|
|
|
Vector3 enter_pos;
|
|
Vector3 exit_pos;
|
|
|
|
if (p_state.length_flat > CMP_EPSILON) {
|
|
real_t flat_to_3d = p_state.length / p_state.length_flat;
|
|
real_t enter_param = p_state.prev_dist * flat_to_3d;
|
|
real_t exit_param = p_state.dist * flat_to_3d;
|
|
enter_pos = p_params.from + p_params.dir * enter_param;
|
|
exit_pos = p_params.from + p_params.dir * exit_param;
|
|
} else {
|
|
// Consider the ray vertical.
|
|
// (though we shouldn't reach this often because there is an early check up-front)
|
|
enter_pos = p_params.from;
|
|
exit_pos = p_params.to;
|
|
}
|
|
|
|
// Transform positions to heightmap space.
|
|
enter_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE;
|
|
exit_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE;
|
|
|
|
// We did enter the flat projection of the AABB,
|
|
// but we have to check if we intersect it on the vertical axis.
|
|
if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) {
|
|
return false;
|
|
}
|
|
if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) {
|
|
return false;
|
|
}
|
|
|
|
return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal);
|
|
}
|
|
|
|
template <typename ProcessFunction>
|
|
bool HeightMapShape3DSW::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const {
|
|
Vector3 delta = (p_end - p_begin);
|
|
real_t length = delta.length();
|
|
|
|
if (length < CMP_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
Vector3 local_begin = p_begin + offset;
|
|
|
|
FaceShape3DSW face;
|
|
face.backface_collision = false;
|
|
|
|
_HeightmapSegmentCullParams params;
|
|
params.from = p_begin;
|
|
params.to = p_end;
|
|
params.dir = delta / length;
|
|
params.heightmap = this;
|
|
params.face = &face;
|
|
|
|
_HeightmapGridCullState state;
|
|
|
|
// Perform grid query from projected ray.
|
|
Vector2 ray_dir_flat(delta.x, delta.z);
|
|
state.length = length;
|
|
state.length_flat = ray_dir_flat.length();
|
|
|
|
if (state.length_flat < CMP_EPSILON) {
|
|
ray_dir_flat = Vector2();
|
|
} else {
|
|
ray_dir_flat /= state.length_flat;
|
|
}
|
|
|
|
const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0);
|
|
const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0);
|
|
|
|
const real_t infinite = 1e20;
|
|
const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite;
|
|
const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite;
|
|
|
|
real_t cross_x; // At which value of `param` we will cross a x-axis lane?
|
|
real_t cross_z; // At which value of `param` we will cross a z-axis lane?
|
|
|
|
// X initialization.
|
|
if (x_step != 0) {
|
|
if (x_step == 1) {
|
|
cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x;
|
|
} else {
|
|
cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x;
|
|
}
|
|
} else {
|
|
cross_x = infinite; // Will never cross on X.
|
|
}
|
|
|
|
// Z initialization.
|
|
if (z_step != 0) {
|
|
if (z_step == 1) {
|
|
cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z;
|
|
} else {
|
|
cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z;
|
|
}
|
|
} else {
|
|
cross_z = infinite; // Will never cross on Z.
|
|
}
|
|
|
|
int x = Math::floor(local_begin.x);
|
|
int z = Math::floor(local_begin.z);
|
|
|
|
// Workaround cases where the ray starts at an integer position.
|
|
if (Math::is_zero_approx(cross_x)) {
|
|
cross_x += delta_x;
|
|
// If going backwards, we should ignore the position we would get by the above flooring,
|
|
// because the ray is not heading in that direction.
|
|
if (x_step == -1) {
|
|
x -= 1;
|
|
}
|
|
}
|
|
|
|
if (Math::is_zero_approx(cross_z)) {
|
|
cross_z += delta_z;
|
|
if (z_step == -1) {
|
|
z -= 1;
|
|
}
|
|
}
|
|
|
|
// Start inside the grid.
|
|
int x_start = MAX(MIN(x, p_width - 2), 0);
|
|
int z_start = MAX(MIN(z, p_depth - 2), 0);
|
|
|
|
// Adjust initial cross values.
|
|
cross_x += delta_x * x_step * (x_start - x);
|
|
cross_z += delta_z * z_step * (z_start - z);
|
|
|
|
x = x_start;
|
|
z = z_start;
|
|
|
|
while (true) {
|
|
state.prev_dist = state.dist;
|
|
state.x = x;
|
|
state.z = z;
|
|
|
|
if (cross_x < cross_z) {
|
|
// X lane.
|
|
x += x_step;
|
|
// Assign before advancing the param,
|
|
// to be in sync with the initialization step.
|
|
state.dist = cross_x;
|
|
cross_x += delta_x;
|
|
} else {
|
|
// Z lane.
|
|
z += z_step;
|
|
state.dist = cross_z;
|
|
cross_z += delta_z;
|
|
}
|
|
|
|
if (state.dist > state.length_flat) {
|
|
state.dist = state.length_flat;
|
|
if (p_process(params, state)) {
|
|
r_point = params.result;
|
|
r_normal = params.normal;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (p_process(params, state)) {
|
|
r_point = params.result;
|
|
r_normal = params.normal;
|
|
return true;
|
|
}
|
|
|
|
// Stop when outside the grid.
|
|
if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
|
|
if (heights.is_empty()) {
|
|
return false;
|
|
}
|
|
|
|
Vector3 local_begin = p_begin + local_origin;
|
|
Vector3 local_end = p_end + local_origin;
|
|
|
|
// Quantize the ray begin/end.
|
|
int begin_x = Math::floor(local_begin.x);
|
|
int begin_z = Math::floor(local_begin.z);
|
|
int end_x = Math::floor(local_end.x);
|
|
int end_z = Math::floor(local_end.z);
|
|
|
|
if ((begin_x == end_x) && (begin_z == end_z)) {
|
|
// Simple case for rays that don't traverse the grid horizontally.
|
|
// Just perform a test on the given cell.
|
|
FaceShape3DSW face;
|
|
face.backface_collision = false;
|
|
|
|
_HeightmapSegmentCullParams params;
|
|
params.from = p_begin;
|
|
params.to = p_end;
|
|
params.dir = (p_end - p_begin).normalized();
|
|
|
|
params.heightmap = this;
|
|
params.face = &face;
|
|
|
|
_HeightmapGridCullState state;
|
|
state.x = MAX(MIN(begin_x, width - 2), 0);
|
|
state.z = MAX(MIN(begin_z, depth - 2), 0);
|
|
if (_heightmap_cell_cull_segment(params, state)) {
|
|
r_point = params.result;
|
|
r_normal = params.normal;
|
|
return true;
|
|
}
|
|
} else if (bounds_grid.is_empty()) {
|
|
// Process all cells intersecting the flat projection of the ray.
|
|
return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
|
|
} else {
|
|
Vector3 ray_diff = (p_end - p_begin);
|
|
real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z;
|
|
if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) {
|
|
// Don't use chunks, the ray is too short in the plane.
|
|
return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
|
|
} else {
|
|
// The ray is long, run raycast on a higher-level grid.
|
|
Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE;
|
|
Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE;
|
|
Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE;
|
|
return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal);
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const {
|
|
return false;
|
|
}
|
|
|
|
Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
|
|
return Vector3();
|
|
}
|
|
|
|
void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
|
|
const AABB &aabb = get_aabb();
|
|
|
|
Vector3 pos_local = aabb.position + local_origin;
|
|
|
|
Vector3 clamped_point(p_point);
|
|
clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x);
|
|
clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y);
|
|
clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z);
|
|
|
|
r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
|
|
r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
|
|
r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
|
|
}
|
|
|
|
void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
|
|
if (heights.is_empty()) {
|
|
return;
|
|
}
|
|
|
|
AABB local_aabb = p_local_aabb;
|
|
local_aabb.position += local_origin;
|
|
|
|
// Quantize the aabb, and adjust the start/end ranges.
|
|
int aabb_min[3];
|
|
int aabb_max[3];
|
|
_get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]);
|
|
_get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]);
|
|
|
|
// Expand the min/max quantized values.
|
|
// This is to catch the case where the input aabb falls between grid points.
|
|
for (int i = 0; i < 3; ++i) {
|
|
aabb_min[i]--;
|
|
aabb_max[i]++;
|
|
}
|
|
|
|
int start_x = MAX(0, aabb_min[0]);
|
|
int end_x = MIN(width - 1, aabb_max[0]);
|
|
int start_z = MAX(0, aabb_min[2]);
|
|
int end_z = MIN(depth - 1, aabb_max[2]);
|
|
|
|
FaceShape3DSW face;
|
|
face.backface_collision = true;
|
|
|
|
for (int z = start_z; z < end_z; z++) {
|
|
for (int x = start_x; x < end_x; x++) {
|
|
// First triangle.
|
|
_get_point(x, z, face.vertex[0]);
|
|
_get_point(x + 1, z, face.vertex[1]);
|
|
_get_point(x, z + 1, face.vertex[2]);
|
|
face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
|
|
if (p_callback(p_userdata, &face)) {
|
|
return;
|
|
}
|
|
|
|
// Second triangle.
|
|
face.vertex[0] = face.vertex[1];
|
|
_get_point(x + 1, z + 1, face.vertex[1]);
|
|
face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
|
|
if (p_callback(p_userdata, &face)) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
|
|
// use bad AABB approximation
|
|
Vector3 extents = get_aabb().size * 0.5;
|
|
|
|
return Vector3(
|
|
(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
|
|
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
|
|
}
|
|
|
|
void HeightMapShape3DSW::_build_accelerator() {
|
|
bounds_grid.clear();
|
|
|
|
bounds_grid_width = width / BOUNDS_CHUNK_SIZE;
|
|
bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE;
|
|
|
|
if (width % BOUNDS_CHUNK_SIZE > 0) {
|
|
++bounds_grid_width; // In case terrain size isn't dividable by chunk size.
|
|
}
|
|
|
|
if (depth % BOUNDS_CHUNK_SIZE > 0) {
|
|
++bounds_grid_depth;
|
|
}
|
|
|
|
uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth);
|
|
|
|
if (bound_grid_size < 2) {
|
|
// Grid is empty or just one chunk.
|
|
return;
|
|
}
|
|
|
|
bounds_grid.resize(bound_grid_size);
|
|
|
|
// Compute min and max height for all chunks.
|
|
for (int cz = 0; cz < bounds_grid_depth; ++cz) {
|
|
int z0 = cz * BOUNDS_CHUNK_SIZE;
|
|
|
|
for (int cx = 0; cx < bounds_grid_width; ++cx) {
|
|
int x0 = cx * BOUNDS_CHUNK_SIZE;
|
|
|
|
Range r;
|
|
|
|
r.min = _get_height(x0, z0);
|
|
r.max = r.min;
|
|
|
|
// Compute min and max height for this chunk.
|
|
// We have to include one extra cell to account for neighbors.
|
|
// Here is why:
|
|
// Say we have a flat terrain, and a plateau that fits a chunk perfectly.
|
|
//
|
|
// Left Right
|
|
// 0---0---0---1---1---1
|
|
// | | | | | |
|
|
// 0---0---0---1---1---1
|
|
// | | | | | |
|
|
// 0---0---0---1---1---1
|
|
// x
|
|
//
|
|
// If the AABB for the Left chunk did not share vertices with the Right,
|
|
// then we would fail collision tests at x due to a gap.
|
|
//
|
|
int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth);
|
|
int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width);
|
|
for (int z = z0; z < z_max; ++z) {
|
|
for (int x = x0; x < x_max; ++x) {
|
|
real_t height = _get_height(x, z);
|
|
if (height < r.min) {
|
|
r.min = height;
|
|
} else if (height > r.max) {
|
|
r.max = height;
|
|
}
|
|
}
|
|
}
|
|
|
|
bounds_grid[cx + cz * bounds_grid_width] = r;
|
|
}
|
|
}
|
|
}
|
|
|
|
void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
|
|
heights = p_heights;
|
|
width = p_width;
|
|
depth = p_depth;
|
|
|
|
// Initialize aabb.
|
|
AABB aabb;
|
|
aabb.position = Vector3(0.0, p_min_height, 0.0);
|
|
aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
|
|
|
|
// Initialize origin as the aabb center.
|
|
local_origin = aabb.position + 0.5 * aabb.size;
|
|
local_origin.y = 0.0;
|
|
|
|
aabb.position -= local_origin;
|
|
|
|
_build_accelerator();
|
|
|
|
configure(aabb);
|
|
}
|
|
|
|
void HeightMapShape3DSW::set_data(const Variant &p_data) {
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
|
|
|
|
Dictionary d = p_data;
|
|
ERR_FAIL_COND(!d.has("width"));
|
|
ERR_FAIL_COND(!d.has("depth"));
|
|
ERR_FAIL_COND(!d.has("heights"));
|
|
|
|
int width = d["width"];
|
|
int depth = d["depth"];
|
|
|
|
ERR_FAIL_COND(width <= 0.0);
|
|
ERR_FAIL_COND(depth <= 0.0);
|
|
|
|
Variant heights_variant = d["heights"];
|
|
Vector<real_t> heights_buffer;
|
|
#ifdef REAL_T_IS_DOUBLE
|
|
if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) {
|
|
#else
|
|
if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) {
|
|
#endif
|
|
// Ready-to-use heights can be passed.
|
|
heights_buffer = heights_variant;
|
|
} else if (heights_variant.get_type() == Variant::OBJECT) {
|
|
// If an image is passed, we have to convert it.
|
|
// This would be expensive to do with a script, so it's nice to have it here.
|
|
Ref<Image> image = heights_variant;
|
|
ERR_FAIL_COND(image.is_null());
|
|
ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF);
|
|
|
|
PackedByteArray im_data = image->get_data();
|
|
heights_buffer.resize(image->get_width() * image->get_height());
|
|
|
|
real_t *w = heights_buffer.ptrw();
|
|
real_t *rp = (real_t *)im_data.ptr();
|
|
for (int i = 0; i < heights_buffer.size(); ++i) {
|
|
w[i] = rp[i];
|
|
}
|
|
} else {
|
|
#ifdef REAL_T_IS_DOUBLE
|
|
ERR_FAIL_MSG("Expected PackedFloat64Array or float Image.");
|
|
#else
|
|
ERR_FAIL_MSG("Expected PackedFloat32Array or float Image.");
|
|
#endif
|
|
}
|
|
|
|
// Compute min and max heights or use precomputed values.
|
|
real_t min_height = 0.0;
|
|
real_t max_height = 0.0;
|
|
if (d.has("min_height") && d.has("max_height")) {
|
|
min_height = d["min_height"];
|
|
max_height = d["max_height"];
|
|
} else {
|
|
int heights_size = heights.size();
|
|
for (int i = 0; i < heights_size; ++i) {
|
|
real_t h = heights[i];
|
|
if (h < min_height) {
|
|
min_height = h;
|
|
} else if (h > max_height) {
|
|
max_height = h;
|
|
}
|
|
}
|
|
}
|
|
|
|
ERR_FAIL_COND(min_height > max_height);
|
|
|
|
ERR_FAIL_COND(heights_buffer.size() != (width * depth));
|
|
|
|
// If specified, min and max height will be used as precomputed values.
|
|
_setup(heights_buffer, width, depth, min_height, max_height);
|
|
}
|
|
|
|
Variant HeightMapShape3DSW::get_data() const {
|
|
Dictionary d;
|
|
d["width"] = width;
|
|
d["depth"] = depth;
|
|
|
|
const AABB &aabb = get_aabb();
|
|
d["min_height"] = aabb.position.y;
|
|
d["max_height"] = aabb.position.y + aabb.size.y;
|
|
|
|
d["heights"] = heights;
|
|
|
|
return d;
|
|
}
|
|
|
|
HeightMapShape3DSW::HeightMapShape3DSW() {
|
|
}
|