a7f49ac9a1
Happy new year to the wonderful Godot community! We're starting a new decade with a well-established, non-profit, free and open source game engine, and tons of further improvements in the pipeline from hundreds of contributors. Godot will keep getting better, and we're looking forward to all the games that the community will keep developing and releasing with it.
1083 lines
25 KiB
C++
1083 lines
25 KiB
C++
/*************************************************************************/
|
|
/* shape_2d_sw.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
|
|
/* Copyright (c) 2014-2020 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_2d_sw.h"
|
|
|
|
#include "core/math/geometry.h"
|
|
#include "core/sort_array.h"
|
|
|
|
void Shape2DSW::configure(const Rect2 &p_aabb) {
|
|
aabb = p_aabb;
|
|
configured = true;
|
|
for (Map<ShapeOwner2DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
|
|
ShapeOwner2DSW *co = (ShapeOwner2DSW *)E->key();
|
|
co->_shape_changed();
|
|
}
|
|
}
|
|
|
|
Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const {
|
|
|
|
Vector2 res[2];
|
|
int amnt;
|
|
get_supports(p_normal, res, amnt);
|
|
return res[0];
|
|
}
|
|
|
|
void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
|
|
|
|
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
|
|
if (E) {
|
|
E->get()++;
|
|
} else {
|
|
owners[p_owner] = 1;
|
|
}
|
|
}
|
|
|
|
void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
|
|
|
|
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
|
|
ERR_FAIL_COND(!E);
|
|
E->get()--;
|
|
if (E->get() == 0) {
|
|
owners.erase(E);
|
|
}
|
|
}
|
|
|
|
bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const {
|
|
|
|
return owners.has(p_owner);
|
|
}
|
|
|
|
const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const {
|
|
return owners;
|
|
}
|
|
|
|
Shape2DSW::Shape2DSW() {
|
|
|
|
custom_bias = 0;
|
|
configured = false;
|
|
}
|
|
|
|
Shape2DSW::~Shape2DSW() {
|
|
|
|
ERR_FAIL_COND(owners.size());
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void LineShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
r_amount = 0;
|
|
}
|
|
|
|
bool LineShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
return normal.dot(p_point) < d;
|
|
}
|
|
|
|
bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
Vector2 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;
|
|
}
|
|
|
|
r_point = p_begin + segment * -dist;
|
|
r_normal = normal;
|
|
|
|
return true;
|
|
}
|
|
|
|
real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
return 0;
|
|
}
|
|
|
|
void LineShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
|
|
Array arr = p_data;
|
|
ERR_FAIL_COND(arr.size() != 2);
|
|
normal = arr[0];
|
|
d = arr[1];
|
|
configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2)));
|
|
}
|
|
|
|
Variant LineShape2DSW::get_data() const {
|
|
|
|
Array arr;
|
|
arr.resize(2);
|
|
arr[0] = normal;
|
|
arr[1] = d;
|
|
return arr;
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
r_amount = 1;
|
|
|
|
if (p_normal.y > 0)
|
|
*r_supports = Vector2(0, length);
|
|
else
|
|
*r_supports = Vector2();
|
|
}
|
|
|
|
bool RayShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
return false;
|
|
}
|
|
|
|
bool RayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
return false; //rays can't be intersected
|
|
}
|
|
|
|
real_t RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
return 0; //rays are mass-less
|
|
}
|
|
|
|
void RayShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
Dictionary d = p_data;
|
|
length = d["length"];
|
|
slips_on_slope = d["slips_on_slope"];
|
|
configure(Rect2(0, 0, 0.001, length));
|
|
}
|
|
|
|
Variant RayShape2DSW::get_data() const {
|
|
|
|
Dictionary d;
|
|
d["length"] = length;
|
|
d["slips_on_slope"] = slips_on_slope;
|
|
return d;
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) {
|
|
r_supports[0] = a;
|
|
r_supports[1] = b;
|
|
r_amount = 2;
|
|
return;
|
|
}
|
|
|
|
real_t dp = p_normal.dot(b - a);
|
|
if (dp > 0)
|
|
*r_supports = b;
|
|
else
|
|
*r_supports = a;
|
|
r_amount = 1;
|
|
}
|
|
|
|
bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
return false;
|
|
}
|
|
|
|
bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point))
|
|
return false;
|
|
|
|
if (n.dot(p_begin) > n.dot(a)) {
|
|
r_normal = n;
|
|
} else {
|
|
r_normal = -n;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
|
|
}
|
|
|
|
void SegmentShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
|
|
|
|
Rect2 r = p_data;
|
|
a = r.position;
|
|
b = r.size;
|
|
n = (b - a).tangent();
|
|
|
|
Rect2 aabb;
|
|
aabb.position = a;
|
|
aabb.expand_to(b);
|
|
if (aabb.size.x == 0)
|
|
aabb.size.x = 0.001;
|
|
if (aabb.size.y == 0)
|
|
aabb.size.y = 0.001;
|
|
configure(aabb);
|
|
}
|
|
|
|
Variant SegmentShape2DSW::get_data() const {
|
|
|
|
Rect2 r;
|
|
r.position = a;
|
|
r.size = b;
|
|
return r;
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
r_amount = 1;
|
|
*r_supports = p_normal * radius;
|
|
}
|
|
|
|
bool CircleShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
return p_point.length_squared() < radius * radius;
|
|
}
|
|
|
|
bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
Vector2 line_vec = p_end - p_begin;
|
|
|
|
real_t a, b, c;
|
|
|
|
a = line_vec.dot(line_vec);
|
|
b = 2 * p_begin.dot(line_vec);
|
|
c = p_begin.dot(p_begin) - radius * radius;
|
|
|
|
real_t sqrtterm = b * b - 4 * a * c;
|
|
|
|
if (sqrtterm < 0)
|
|
return false;
|
|
sqrtterm = Math::sqrt(sqrtterm);
|
|
real_t res = (-b - sqrtterm) / (2 * a);
|
|
|
|
if (res < 0 || res > 1 + CMP_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
r_point = p_begin + line_vec * res;
|
|
r_normal = r_point.normalized();
|
|
return true;
|
|
}
|
|
|
|
real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
real_t a = radius * p_scale.x;
|
|
real_t b = radius * p_scale.y;
|
|
return p_mass * (a * a + b * b) / 4;
|
|
}
|
|
|
|
void CircleShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(!p_data.is_num());
|
|
radius = p_data;
|
|
configure(Rect2(-radius, -radius, radius * 2, radius * 2));
|
|
}
|
|
|
|
Variant CircleShape2DSW::get_data() const {
|
|
|
|
return radius;
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
|
|
Vector2 ag;
|
|
ag[i] = 1.0;
|
|
real_t dp = ag.dot(p_normal);
|
|
if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)
|
|
continue;
|
|
|
|
real_t sgn = dp > 0 ? 1.0 : -1.0;
|
|
|
|
r_amount = 2;
|
|
|
|
r_supports[0][i] = half_extents[i] * sgn;
|
|
r_supports[0][i ^ 1] = half_extents[i ^ 1];
|
|
|
|
r_supports[1][i] = half_extents[i] * sgn;
|
|
r_supports[1][i ^ 1] = -half_extents[i ^ 1];
|
|
|
|
return;
|
|
}
|
|
|
|
/* USE POINT */
|
|
|
|
r_amount = 1;
|
|
r_supports[0] = Vector2(
|
|
(p_normal.x < 0) ? -half_extents.x : half_extents.x,
|
|
(p_normal.y < 0) ? -half_extents.y : half_extents.y);
|
|
}
|
|
|
|
bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
float x = p_point.x;
|
|
float y = p_point.y;
|
|
float edge_x = half_extents.x;
|
|
float edge_y = half_extents.y;
|
|
return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
|
|
}
|
|
|
|
bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
|
|
}
|
|
|
|
real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
Vector2 he2 = half_extents * 2 * p_scale;
|
|
return p_mass * he2.dot(he2) / 12.0;
|
|
}
|
|
|
|
void RectangleShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
|
|
|
|
half_extents = p_data;
|
|
configure(Rect2(-half_extents, half_extents * 2.0));
|
|
}
|
|
|
|
Variant RectangleShape2DSW::get_data() const {
|
|
|
|
return half_extents;
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
Vector2 n = p_normal;
|
|
|
|
real_t d = n.y;
|
|
|
|
if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) {
|
|
|
|
// make it flat
|
|
n.y = 0.0;
|
|
n.normalize();
|
|
n *= radius;
|
|
|
|
r_amount = 2;
|
|
r_supports[0] = n;
|
|
r_supports[0].y += height * 0.5;
|
|
r_supports[1] = n;
|
|
r_supports[1].y -= height * 0.5;
|
|
|
|
} else {
|
|
|
|
real_t h = (d > 0) ? height : -height;
|
|
|
|
n *= radius;
|
|
n.y += h * 0.5;
|
|
r_amount = 1;
|
|
*r_supports = n;
|
|
}
|
|
}
|
|
|
|
bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
Vector2 p = p_point;
|
|
p.y = Math::abs(p.y);
|
|
p.y -= height * 0.5;
|
|
if (p.y < 0)
|
|
p.y = 0;
|
|
|
|
return p.length_squared() < radius * radius;
|
|
}
|
|
|
|
bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
real_t d = 1e10;
|
|
Vector2 n = (p_end - p_begin).normalized();
|
|
bool collided = false;
|
|
|
|
//try spheres
|
|
for (int i = 0; i < 2; i++) {
|
|
|
|
Vector2 begin = p_begin;
|
|
Vector2 end = p_end;
|
|
real_t ofs = (i == 0) ? -height * 0.5 : height * 0.5;
|
|
begin.y += ofs;
|
|
end.y += ofs;
|
|
|
|
Vector2 line_vec = end - begin;
|
|
|
|
real_t a, b, c;
|
|
|
|
a = line_vec.dot(line_vec);
|
|
b = 2 * begin.dot(line_vec);
|
|
c = begin.dot(begin) - radius * radius;
|
|
|
|
real_t sqrtterm = b * b - 4 * a * c;
|
|
|
|
if (sqrtterm < 0)
|
|
continue;
|
|
|
|
sqrtterm = Math::sqrt(sqrtterm);
|
|
real_t res = (-b - sqrtterm) / (2 * a);
|
|
|
|
if (res < 0 || res > 1 + CMP_EPSILON) {
|
|
continue;
|
|
}
|
|
|
|
Vector2 point = begin + line_vec * res;
|
|
Vector2 pointf(point.x, point.y - ofs);
|
|
real_t pd = n.dot(pointf);
|
|
if (pd < d) {
|
|
r_point = pointf;
|
|
r_normal = point.normalized();
|
|
d = pd;
|
|
collided = true;
|
|
}
|
|
}
|
|
|
|
Vector2 rpos, rnorm;
|
|
if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
|
|
|
|
real_t pd = n.dot(rpos);
|
|
if (pd < d) {
|
|
r_point = rpos;
|
|
r_normal = rnorm;
|
|
d = pd;
|
|
collided = true;
|
|
}
|
|
}
|
|
|
|
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
|
|
return collided; //todo
|
|
}
|
|
|
|
real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale;
|
|
return p_mass * he2.dot(he2) / 12.0;
|
|
}
|
|
|
|
void CapsuleShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
|
|
|
|
if (p_data.get_type() == Variant::ARRAY) {
|
|
Array arr = p_data;
|
|
ERR_FAIL_COND(arr.size() != 2);
|
|
height = arr[0];
|
|
radius = arr[1];
|
|
} else {
|
|
|
|
Point2 p = p_data;
|
|
radius = p.x;
|
|
height = p.y;
|
|
}
|
|
|
|
Point2 he(radius, height * 0.5 + radius);
|
|
configure(Rect2(-he, he * 2));
|
|
}
|
|
|
|
Variant CapsuleShape2DSW::get_data() const {
|
|
|
|
return Point2(height, radius);
|
|
}
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
int support_idx = -1;
|
|
real_t d = -1e10;
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
//test point
|
|
real_t ld = p_normal.dot(points[i].pos);
|
|
if (ld > d) {
|
|
support_idx = i;
|
|
d = ld;
|
|
}
|
|
|
|
//test segment
|
|
if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) {
|
|
|
|
r_amount = 2;
|
|
r_supports[0] = points[i].pos;
|
|
r_supports[1] = points[(i + 1) % point_count].pos;
|
|
return;
|
|
}
|
|
}
|
|
|
|
ERR_FAIL_COND(support_idx == -1);
|
|
|
|
r_amount = 1;
|
|
r_supports[0] = points[support_idx].pos;
|
|
}
|
|
|
|
bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
bool out = false;
|
|
bool in = false;
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
|
|
if (d > 0)
|
|
out = true;
|
|
else
|
|
in = true;
|
|
}
|
|
|
|
return in != out;
|
|
}
|
|
|
|
bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
Vector2 n = (p_end - p_begin).normalized();
|
|
real_t d = 1e10;
|
|
bool inters = false;
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
//hmm.. no can do..
|
|
/*
|
|
if (d.dot(points[i].normal)>=0)
|
|
continue;
|
|
*/
|
|
|
|
Vector2 res;
|
|
|
|
if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res))
|
|
continue;
|
|
|
|
real_t nd = n.dot(res);
|
|
if (nd < d) {
|
|
|
|
d = nd;
|
|
r_point = res;
|
|
r_normal = points[i].normal;
|
|
inters = true;
|
|
}
|
|
}
|
|
|
|
if (inters) {
|
|
|
|
if (n.dot(r_normal) > 0)
|
|
r_normal = -r_normal;
|
|
}
|
|
|
|
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
|
|
return inters; //todo
|
|
}
|
|
|
|
real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
|
|
|
|
Rect2 aabb;
|
|
aabb.position = points[0].pos * p_scale;
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
aabb.expand_to(points[i].pos * p_scale);
|
|
}
|
|
|
|
return p_mass * aabb.size.dot(aabb.size) / 12.0;
|
|
}
|
|
|
|
void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::POOL_VECTOR2_ARRAY && p_data.get_type() != Variant::POOL_REAL_ARRAY);
|
|
|
|
if (points)
|
|
memdelete_arr(points);
|
|
points = NULL;
|
|
point_count = 0;
|
|
|
|
if (p_data.get_type() == Variant::POOL_VECTOR2_ARRAY) {
|
|
PoolVector<Vector2> arr = p_data;
|
|
ERR_FAIL_COND(arr.size() == 0);
|
|
point_count = arr.size();
|
|
points = memnew_arr(Point, point_count);
|
|
PoolVector<Vector2>::Read r = arr.read();
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
points[i].pos = r[i];
|
|
}
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
Vector2 p = points[i].pos;
|
|
Vector2 pn = points[(i + 1) % point_count].pos;
|
|
points[i].normal = (pn - p).tangent().normalized();
|
|
}
|
|
} else {
|
|
|
|
PoolVector<real_t> dvr = p_data;
|
|
point_count = dvr.size() / 4;
|
|
ERR_FAIL_COND(point_count == 0);
|
|
|
|
points = memnew_arr(Point, point_count);
|
|
PoolVector<real_t>::Read r = dvr.read();
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
|
|
int idx = i << 2;
|
|
points[i].pos.x = r[idx + 0];
|
|
points[i].pos.y = r[idx + 1];
|
|
points[i].normal.x = r[idx + 2];
|
|
points[i].normal.y = r[idx + 3];
|
|
}
|
|
}
|
|
|
|
ERR_FAIL_COND(point_count == 0);
|
|
Rect2 aabb;
|
|
aabb.position = points[0].pos;
|
|
for (int i = 1; i < point_count; i++)
|
|
aabb.expand_to(points[i].pos);
|
|
|
|
configure(aabb);
|
|
}
|
|
|
|
Variant ConvexPolygonShape2DSW::get_data() const {
|
|
|
|
PoolVector<Vector2> dvr;
|
|
|
|
dvr.resize(point_count);
|
|
|
|
for (int i = 0; i < point_count; i++) {
|
|
dvr.set(i, points[i].pos);
|
|
}
|
|
|
|
return dvr;
|
|
}
|
|
|
|
ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
|
|
|
|
points = NULL;
|
|
point_count = 0;
|
|
}
|
|
|
|
ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() {
|
|
|
|
if (points)
|
|
memdelete_arr(points);
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
|
|
void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
|
|
|
|
real_t d = -1e10;
|
|
int idx = -1;
|
|
for (int i = 0; i < points.size(); i++) {
|
|
|
|
real_t ld = p_normal.dot(points[i]);
|
|
if (ld > d) {
|
|
d = ld;
|
|
idx = i;
|
|
}
|
|
}
|
|
|
|
r_amount = 1;
|
|
ERR_FAIL_COND(idx == -1);
|
|
*r_supports = points[idx];
|
|
}
|
|
|
|
bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const {
|
|
|
|
return false; //sorry
|
|
}
|
|
|
|
bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
|
|
|
|
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
|
|
|
|
enum {
|
|
TEST_AABB_BIT = 0,
|
|
VISIT_LEFT_BIT = 1,
|
|
VISIT_RIGHT_BIT = 2,
|
|
VISIT_DONE_BIT = 3,
|
|
VISITED_BIT_SHIFT = 29,
|
|
NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
|
|
VISITED_BIT_MASK = ~NODE_IDX_MASK,
|
|
|
|
};
|
|
|
|
Vector2 n = (p_end - p_begin).normalized();
|
|
real_t d = 1e10;
|
|
bool inters = false;
|
|
|
|
/*
|
|
for(int i=0;i<bvh_depth;i++)
|
|
stack[i]=0;
|
|
*/
|
|
|
|
int level = 0;
|
|
|
|
const Segment *segmentptr = &segments[0];
|
|
const Vector2 *pointptr = &points[0];
|
|
const BVH *bvhptr = &bvh[0];
|
|
|
|
stack[0] = 0;
|
|
while (true) {
|
|
|
|
uint32_t node = stack[level] & NODE_IDX_MASK;
|
|
const BVH &bvh = bvhptr[node];
|
|
bool done = false;
|
|
|
|
switch (stack[level] >> VISITED_BIT_SHIFT) {
|
|
case TEST_AABB_BIT: {
|
|
|
|
bool valid = bvh.aabb.intersects_segment(p_begin, p_end);
|
|
if (!valid) {
|
|
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
|
|
} else {
|
|
|
|
if (bvh.left < 0) {
|
|
|
|
const Segment &s = segmentptr[bvh.right];
|
|
Vector2 a = pointptr[s.points[0]];
|
|
Vector2 b = pointptr[s.points[1]];
|
|
|
|
Vector2 res;
|
|
|
|
if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) {
|
|
|
|
real_t nd = n.dot(res);
|
|
if (nd < d) {
|
|
|
|
d = nd;
|
|
r_point = res;
|
|
r_normal = (b - a).tangent().normalized();
|
|
inters = true;
|
|
}
|
|
}
|
|
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
|
|
} else {
|
|
|
|
stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
|
|
}
|
|
}
|
|
}
|
|
continue;
|
|
case VISIT_LEFT_BIT: {
|
|
|
|
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
|
|
stack[level + 1] = bvh.left | TEST_AABB_BIT;
|
|
level++;
|
|
}
|
|
continue;
|
|
case VISIT_RIGHT_BIT: {
|
|
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
stack[level + 1] = bvh.right | TEST_AABB_BIT;
|
|
level++;
|
|
}
|
|
continue;
|
|
case VISIT_DONE_BIT: {
|
|
|
|
if (level == 0) {
|
|
done = true;
|
|
break;
|
|
} else
|
|
level--;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (done)
|
|
break;
|
|
}
|
|
|
|
if (inters) {
|
|
|
|
if (n.dot(r_normal) > 0)
|
|
r_normal = -r_normal;
|
|
}
|
|
|
|
return inters;
|
|
}
|
|
|
|
int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
|
|
|
|
if (p_len == 1) {
|
|
|
|
bvh_depth = MAX(p_depth, bvh_depth);
|
|
bvh.push_back(*p_bvh);
|
|
return bvh.size() - 1;
|
|
}
|
|
|
|
//else sort best
|
|
|
|
Rect2 global_aabb = p_bvh[0].aabb;
|
|
for (int i = 1; i < p_len; i++) {
|
|
global_aabb = global_aabb.merge(p_bvh[i].aabb);
|
|
}
|
|
|
|
if (global_aabb.size.x > global_aabb.size.y) {
|
|
|
|
SortArray<BVH, BVH_CompareX> sort;
|
|
sort.sort(p_bvh, p_len);
|
|
|
|
} else {
|
|
|
|
SortArray<BVH, BVH_CompareY> sort;
|
|
sort.sort(p_bvh, p_len);
|
|
}
|
|
|
|
int median = p_len / 2;
|
|
|
|
BVH node;
|
|
node.aabb = global_aabb;
|
|
int node_idx = bvh.size();
|
|
bvh.push_back(node);
|
|
|
|
int l = _generate_bvh(p_bvh, median, p_depth + 1);
|
|
int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1);
|
|
bvh.write[node_idx].left = l;
|
|
bvh.write[node_idx].right = r;
|
|
|
|
return node_idx;
|
|
}
|
|
|
|
void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type() != Variant::POOL_VECTOR2_ARRAY && p_data.get_type() != Variant::POOL_REAL_ARRAY);
|
|
|
|
Rect2 aabb;
|
|
|
|
if (p_data.get_type() == Variant::POOL_VECTOR2_ARRAY) {
|
|
|
|
PoolVector<Vector2> p2arr = p_data;
|
|
int len = p2arr.size();
|
|
ERR_FAIL_COND(len % 2);
|
|
|
|
segments.clear();
|
|
points.clear();
|
|
bvh.clear();
|
|
bvh_depth = 1;
|
|
|
|
if (len == 0) {
|
|
configure(aabb);
|
|
return;
|
|
}
|
|
|
|
PoolVector<Vector2>::Read arr = p2arr.read();
|
|
|
|
Map<Point2, int> pointmap;
|
|
for (int i = 0; i < len; i += 2) {
|
|
|
|
Point2 p1 = arr[i];
|
|
Point2 p2 = arr[i + 1];
|
|
int idx_p1, idx_p2;
|
|
|
|
if (pointmap.has(p1)) {
|
|
idx_p1 = pointmap[p1];
|
|
} else {
|
|
idx_p1 = pointmap.size();
|
|
pointmap[p1] = idx_p1;
|
|
}
|
|
|
|
if (pointmap.has(p2)) {
|
|
idx_p2 = pointmap[p2];
|
|
} else {
|
|
idx_p2 = pointmap.size();
|
|
pointmap[p2] = idx_p2;
|
|
}
|
|
|
|
Segment s;
|
|
s.points[0] = idx_p1;
|
|
s.points[1] = idx_p2;
|
|
segments.push_back(s);
|
|
}
|
|
|
|
points.resize(pointmap.size());
|
|
aabb.position = pointmap.front()->key();
|
|
for (Map<Point2, int>::Element *E = pointmap.front(); E; E = E->next()) {
|
|
|
|
aabb.expand_to(E->key());
|
|
points.write[E->get()] = E->key();
|
|
}
|
|
|
|
Vector<BVH> main_vbh;
|
|
main_vbh.resize(segments.size());
|
|
for (int i = 0; i < main_vbh.size(); i++) {
|
|
|
|
main_vbh.write[i].aabb.position = points[segments[i].points[0]];
|
|
main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]);
|
|
main_vbh.write[i].left = -1;
|
|
main_vbh.write[i].right = i;
|
|
}
|
|
|
|
_generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1);
|
|
|
|
} else {
|
|
//dictionary with arrays
|
|
}
|
|
|
|
configure(aabb);
|
|
}
|
|
Variant ConcavePolygonShape2DSW::get_data() const {
|
|
|
|
PoolVector<Vector2> rsegments;
|
|
int len = segments.size();
|
|
rsegments.resize(len * 2);
|
|
PoolVector<Vector2>::Write w = rsegments.write();
|
|
for (int i = 0; i < len; i++) {
|
|
|
|
w[(i << 1) + 0] = points[segments[i].points[0]];
|
|
w[(i << 1) + 1] = points[segments[i].points[1]];
|
|
}
|
|
|
|
w.release();
|
|
|
|
return rsegments;
|
|
}
|
|
|
|
void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const {
|
|
|
|
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
|
|
|
|
enum {
|
|
TEST_AABB_BIT = 0,
|
|
VISIT_LEFT_BIT = 1,
|
|
VISIT_RIGHT_BIT = 2,
|
|
VISIT_DONE_BIT = 3,
|
|
VISITED_BIT_SHIFT = 29,
|
|
NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
|
|
VISITED_BIT_MASK = ~NODE_IDX_MASK,
|
|
|
|
};
|
|
|
|
/*
|
|
for(int i=0;i<bvh_depth;i++)
|
|
stack[i]=0;
|
|
*/
|
|
|
|
if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) {
|
|
return;
|
|
}
|
|
|
|
int level = 0;
|
|
|
|
const Segment *segmentptr = &segments[0];
|
|
const Vector2 *pointptr = &points[0];
|
|
const BVH *bvhptr = &bvh[0];
|
|
|
|
stack[0] = 0;
|
|
while (true) {
|
|
|
|
uint32_t node = stack[level] & NODE_IDX_MASK;
|
|
const BVH &bvh = bvhptr[node];
|
|
|
|
switch (stack[level] >> VISITED_BIT_SHIFT) {
|
|
case TEST_AABB_BIT: {
|
|
|
|
bool valid = p_local_aabb.intersects(bvh.aabb);
|
|
if (!valid) {
|
|
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
|
|
} else {
|
|
|
|
if (bvh.left < 0) {
|
|
|
|
const Segment &s = segmentptr[bvh.right];
|
|
Vector2 a = pointptr[s.points[0]];
|
|
Vector2 b = pointptr[s.points[1]];
|
|
|
|
SegmentShape2DSW ss(a, b, (b - a).tangent().normalized());
|
|
|
|
p_callback(p_userdata, &ss);
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
|
|
} else {
|
|
|
|
stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
|
|
}
|
|
}
|
|
}
|
|
continue;
|
|
case VISIT_LEFT_BIT: {
|
|
|
|
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
|
|
stack[level + 1] = bvh.left | TEST_AABB_BIT;
|
|
level++;
|
|
}
|
|
continue;
|
|
case VISIT_RIGHT_BIT: {
|
|
|
|
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
|
|
stack[level + 1] = bvh.right | TEST_AABB_BIT;
|
|
level++;
|
|
}
|
|
continue;
|
|
case VISIT_DONE_BIT: {
|
|
|
|
if (level == 0)
|
|
return;
|
|
else
|
|
level--;
|
|
}
|
|
continue;
|
|
}
|
|
}
|
|
}
|