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-2016 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 Vector2 &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 Vector2 &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;
|
|
|
|
Vector2 d = p_end-p_begin;
|
|
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 Vector2 &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 Vector2 &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 Vector2& 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 Vector2 &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 Vector2& 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];
|
|
int pos=bvh.size()-1;
|
|
|
|
|
|
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);
|
|
|
|
segments.clear();;
|
|
points.clear();;
|
|
bvh.clear();;
|
|
bvh_depth=1;
|
|
|
|
Rect2 aabb;
|
|
|
|
if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
|
|
|
|
DVector<Vector2> p2arr = p_data;
|
|
int len = p2arr.size();
|
|
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 (p1==p2)
|
|
continue; //don't want it
|
|
|
|
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];
|
|
int pos=bvh.size()-1;
|
|
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|