d8223ffa75
That year should bring the long-awaited OpenGL ES 3.0 compatible renderer
with state-of-the-art rendering techniques tuned to work as low as middle
end handheld devices - without compromising with the possibilities given
for higher end desktop games of course. Great times ahead for the Godot
community and the gamers that will play our games!
(cherry picked from commit c7bc44d5ad)
1152 lines
23 KiB
C++
1152 lines
23 KiB
C++
/*************************************************************************/
|
|
/* shape_2d_sw.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* http://www.godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/*************************************************************************/
|
|
#include "shape_2d_sw.h"
|
|
#include "geometry.h"
|
|
#include "sort.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(float 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(float p_mass, const Size2 &p_scale) const {
|
|
|
|
return 0; //rays are mass-less
|
|
}
|
|
|
|
void RayShape2DSW::set_data(const Variant& p_data) {
|
|
|
|
length=p_data;
|
|
configure(Rect2(0,0,0.001,length));
|
|
}
|
|
|
|
Variant RayShape2DSW::get_data() const {
|
|
|
|
return length;
|
|
}
|
|
|
|
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
/*********************************************************/
|
|
|
|
|
|
|
|
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_TRESHOLD) {
|
|
r_supports[0]=a;
|
|
r_supports[1]=b;
|
|
r_amount=2;
|
|
return;
|
|
|
|
}
|
|
|
|
float 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(float p_mass, const Size2 &p_scale) const {
|
|
|
|
Vector2 s[2]={a*p_scale,b*p_scale};
|
|
|
|
real_t l = s[1].distance_to(s[0]);
|
|
Vector2 ofs = (s[0]+s[1])*0.5;
|
|
|
|
return p_mass*(l*l/12.0f + ofs.length_squared());
|
|
}
|
|
|
|
void SegmentShape2DSW::set_data(const Variant& p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
|
|
|
|
Rect2 r = p_data;
|
|
a=r.pos;
|
|
b=r.size;
|
|
n=(b-a).tangent();
|
|
|
|
Rect2 aabb;
|
|
aabb.pos=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.pos=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(float p_mass, const Size2 &p_scale) const {
|
|
|
|
return (radius*radius)*(p_scale.x*0.5+p_scale.y*0.5);
|
|
|
|
}
|
|
|
|
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;
|
|
float dp = ag.dot(p_normal);
|
|
if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
|
|
continue;
|
|
|
|
float 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 {
|
|
|
|
return Math::abs(p_point.x)<half_extents.x && Math::abs(p_point.y)<half_extents.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(float p_mass,const Size2& p_scale) const {
|
|
|
|
Vector2 he2=half_extents*2*p_scale;
|
|
return p_mass*he2.dot(he2)/12.0f;
|
|
}
|
|
|
|
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;
|
|
|
|
float d = n.y;
|
|
|
|
if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
|
|
|
|
// 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 {
|
|
|
|
float 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 {
|
|
|
|
|
|
float 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;
|
|
float 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(float p_mass, const Size2 &p_scale) const {
|
|
|
|
Vector2 he2=Vector2(radius*2,height+radius*2)*p_scale;
|
|
return p_mass*he2.dot(he2)/12.0f;
|
|
}
|
|
|
|
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_TRESHOLD) {
|
|
|
|
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++) {
|
|
|
|
float 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) || (!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 crap.. 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;
|
|
|
|
float 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(float p_mass,const Size2& p_scale) const {
|
|
|
|
Rect2 aabb;
|
|
aabb.pos=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.0f + p_mass * (aabb.pos+aabb.size*0.5).length_squared();
|
|
}
|
|
|
|
void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
|
|
|
|
|
|
if (points)
|
|
memdelete_arr(points);
|
|
points=NULL;
|
|
point_count=0;
|
|
|
|
if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
|
|
DVector<Vector2> arr=p_data;
|
|
ERR_FAIL_COND(arr.size()==0);
|
|
point_count=arr.size();
|
|
points = memnew_arr(Point,point_count);
|
|
DVector<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 {
|
|
|
|
DVector<real_t> dvr = p_data;
|
|
point_count=dvr.size()/4;
|
|
ERR_FAIL_COND(point_count==0);
|
|
|
|
points = memnew_arr(Point,point_count);
|
|
DVector<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.pos=points[0].pos;
|
|
for(int i=1;i<point_count;i++)
|
|
aabb.expand_to(points[i].pos);
|
|
|
|
configure(aabb);
|
|
}
|
|
|
|
Variant ConvexPolygonShape2DSW::get_data() const {
|
|
|
|
DVector<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 &b = bvhptr[ node ];
|
|
bool done=false;
|
|
|
|
switch(stack[level]>>VISITED_BIT_SHIFT) {
|
|
case TEST_AABB_BIT: {
|
|
|
|
|
|
bool valid = b.aabb.intersects_segment(p_begin,p_end);
|
|
if (!valid) {
|
|
|
|
stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
|
|
|
|
} else {
|
|
|
|
if (b.left<0) {
|
|
|
|
const Segment &s=segmentptr[ b.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)) {
|
|
|
|
float 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]=b.left|TEST_AABB_BIT;
|
|
level++;
|
|
|
|
} continue;
|
|
case VISIT_RIGHT_BIT: {
|
|
|
|
stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
|
|
stack[level+1]=b.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[node_idx].left=l;
|
|
bvh[node_idx].right=r;
|
|
|
|
return node_idx;
|
|
|
|
}
|
|
|
|
void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
|
|
|
|
ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
|
|
|
|
Rect2 aabb;
|
|
|
|
if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
|
|
|
|
DVector<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;
|
|
}
|
|
|
|
DVector<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.pos=pointmap.front()->key();
|
|
for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
|
|
|
|
aabb.expand_to(E->key());
|
|
points[E->get()]=E->key();
|
|
}
|
|
|
|
Vector<BVH> main_vbh;
|
|
main_vbh.resize(segments.size());
|
|
for(int i=0;i<main_vbh.size();i++) {
|
|
|
|
|
|
main_vbh[i].aabb.pos=points[segments[i].points[0]];
|
|
main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
|
|
main_vbh[i].left=-1;
|
|
main_vbh[i].right=i;
|
|
}
|
|
|
|
_generate_bvh(&main_vbh[0],main_vbh.size(),1);
|
|
|
|
|
|
} else {
|
|
//dictionary with arrays
|
|
|
|
}
|
|
|
|
|
|
configure(aabb);
|
|
}
|
|
Variant ConcavePolygonShape2DSW::get_data() const {
|
|
|
|
|
|
DVector<Vector2> rsegments;
|
|
int len = segments.size();
|
|
rsegments.resize(len*2);
|
|
DVector<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=DVector<Vector2>::Write();
|
|
|
|
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;
|
|
|
|
|
|
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 &b = bvhptr[ node ];
|
|
|
|
switch(stack[level]>>VISITED_BIT_SHIFT) {
|
|
case TEST_AABB_BIT: {
|
|
|
|
|
|
bool valid = p_local_aabb.intersects(b.aabb);
|
|
if (!valid) {
|
|
|
|
stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
|
|
|
|
} else {
|
|
|
|
if (b.left<0) {
|
|
|
|
const Segment &s=segmentptr[ b.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]=b.left|TEST_AABB_BIT;
|
|
level++;
|
|
|
|
} continue;
|
|
case VISIT_RIGHT_BIT: {
|
|
|
|
stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
|
|
stack[level+1]=b.right|TEST_AABB_BIT;
|
|
level++;
|
|
} continue;
|
|
case VISIT_DONE_BIT: {
|
|
|
|
if (level==0)
|
|
return;
|
|
else
|
|
level--;
|
|
|
|
} continue;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|