diff options
Diffstat (limited to 'servers/physics/shape_sw.cpp')
| -rw-r--r-- | servers/physics/shape_sw.cpp | 1195 |
1 files changed, 528 insertions, 667 deletions
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index dec1c75d9f..4ce716c70a 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -28,100 +28,91 @@ /*************************************************************************/ #include "shape_sw.h" #include "geometry.h" -#include "sort.h" #include "quick_hull.h" +#include "sort.h" #define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998 - -void ShapeSW::configure(const Rect3& p_aabb) { - aabb=p_aabb; - configured=true; - for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) { - ShapeOwnerSW* co=(ShapeOwnerSW*)E->key(); +void ShapeSW::configure(const Rect3 &p_aabb) { + aabb = p_aabb; + configured = true; + for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) { + ShapeOwnerSW *co = (ShapeOwnerSW *)E->key(); co->_shape_changed(); } } - -Vector3 ShapeSW::get_support(const Vector3& p_normal) const { +Vector3 ShapeSW::get_support(const Vector3 &p_normal) const { Vector3 res; int amnt; - get_supports(p_normal,1,&res,amnt); + get_supports(p_normal, 1, &res, amnt); return res; } void ShapeSW::add_owner(ShapeOwnerSW *p_owner) { - Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); + Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner); if (E) { E->get()++; } else { - owners[p_owner]=1; + owners[p_owner] = 1; } } -void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){ +void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) { - Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner); + Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner); ERR_FAIL_COND(!E); E->get()--; - if (E->get()==0) { + if (E->get() == 0) { owners.erase(E); } - } -bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{ +bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const { return owners.has(p_owner); - } -const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{ +const Map<ShapeOwnerSW *, int> &ShapeSW::get_owners() const { return owners; } - ShapeSW::ShapeSW() { - custom_bias=0; - configured=false; + custom_bias = 0; + configured = false; } - ShapeSW::~ShapeSW() { ERR_FAIL_COND(owners.size()); } - - Plane PlaneShapeSW::get_plane() const { return plane; } -void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity - r_min=-1e7; - r_max=1e7; + r_min = -1e7; + r_max = 1e7; } -Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const { +Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal*1e15; + return p_normal * 1e15; } +bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { -bool PlaneShapeSW::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; + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) + r_normal = plane.normal; return inters; } @@ -130,16 +121,15 @@ Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); //wtf } -void PlaneShapeSW::_setup(const Plane& p_plane) { +void PlaneShapeSW::_setup(const Plane &p_plane) { - plane=p_plane; - configure(Rect3(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2))); + plane = p_plane; + configure(Rect3(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } -void PlaneShapeSW::set_data(const Variant& p_data) { +void PlaneShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant PlaneShapeSW::get_data() const { @@ -147,9 +137,7 @@ Variant PlaneShapeSW::get_data() const { return plane; } -PlaneShapeSW::PlaneShapeSW() { - - +PlaneShapeSW::PlaneShapeSW() { } // @@ -159,39 +147,38 @@ real_t RayShapeSW::get_length() const { return length; } -void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &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; + r_min = 0; + r_max = 1; } -Vector3 RayShapeSW::get_support(const Vector3& p_normal) const { +Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const { - if (p_normal.z>0) - return Vector3(0,0,length); + if (p_normal.z > 0) + return Vector3(0, 0, length); else - return Vector3(0,0,0); + return Vector3(0, 0, 0); } -void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount=2; - r_supports[0]=Vector3(0,0,0); - r_supports[1]=Vector3(0,0,length); - } else if (p_normal.z>0) { - r_amount=1; - *r_supports=Vector3(0,0,length); + r_amount = 2; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + *r_supports = Vector3(0, 0, length); } else { - r_amount=1; - *r_supports=Vector3(0,0,0); + r_amount = 1; + *r_supports = Vector3(0, 0, 0); } } - -bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; //simply not possible } @@ -201,16 +188,15 @@ Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); } -void RayShapeSW::_setup(real_t p_length) { +void RayShapeSW::_setup(real_t p_length) { - length=p_length; - configure(Rect3(Vector3(0,0,0),Vector3(0.1,0.1,length))); + length = p_length; + configure(Rect3(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); } -void RayShapeSW::set_data(const Variant& p_data) { +void RayShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant RayShapeSW::get_data() const { @@ -218,13 +204,11 @@ Variant RayShapeSW::get_data() const { return length; } -RayShapeSW::RayShapeSW() { +RayShapeSW::RayShapeSW() { - length=1; + length = 1; } - - /********** SPHERE *************/ real_t SphereShapeSW::get_radius() const { @@ -232,50 +216,47 @@ real_t SphereShapeSW::get_radius() const { return radius; } -void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - real_t d = p_normal.dot( p_transform.origin ); + 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; - + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; } -Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const { +Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const { - return p_normal*radius; + return p_normal * radius; } -void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - *r_supports=p_normal*radius; - r_amount=1; + *r_supports = p_normal * radius; + r_amount = 1; } -bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal); + return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t s = 0.4 * p_mass * radius * radius; - return Vector3(s,s,s); + return Vector3(s, s, s); } void SphereShapeSW::_setup(real_t p_radius) { - - radius=p_radius; - configure(Rect3( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0))); - + radius = p_radius; + configure(Rect3(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); } -void SphereShapeSW::set_data(const Variant& p_data) { +void SphereShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -287,113 +268,105 @@ Variant SphereShapeSW::get_data() const { SphereShapeSW::SphereShapeSW() { - radius=0; + radius = 0; } - /********** BOX *************/ - -void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &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); + 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 ); + real_t distance = p_normal.dot(p_transform.origin); r_min = distance - length; r_max = distance + length; - - } -Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 BoxShapeSW::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 - ); + (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 BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - static const int next[3]={1,2,0}; - static const int next2[3]={2,0,1}; + static const int next[3] = { 1, 2, 0 }; + static const int next2[3] = { 2, 0, 1 }; - for (int i=0;i<3;i++) { + 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_TRESHOLD ) { + axis[i] = 1.0; + real_t dot = p_normal.dot(axis); + if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { //Vector3 axis_b; - bool neg = dot<0; + bool neg = dot < 0; r_amount = 4; Vector3 point; - point[i]=half_extents[i]; + point[i] = half_extents[i]; - int i_n=next[i]; - int i_n2=next2[i]; + int i_n = next[i]; + int i_n2 = next2[i]; - static const real_t sign[4][2]={ + static const real_t sign[4][2] = { - {-1.0, 1.0}, - { 1.0, 1.0}, - { 1.0,-1.0}, - {-1.0,-1.0}, + { -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; + 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] ); + SWAP(r_supports[1], r_supports[2]); + SWAP(r_supports[0], r_supports[3]); } return; } - r_amount=0; - + r_amount = 0; } - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { Vector3 axis; - axis[i]=1.0; + axis[i] = 1.0; - if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount= 2; + r_amount = 2; - int i_n=next[i]; - int i_n2=next2[i]; + int i_n = next[i]; + int i_n2 = next2[i]; - Vector3 point=half_extents; + Vector3 point = half_extents; - if (p_normal[i_n]<0) { - point[i_n]=-point[i_n]; + if (p_normal[i_n] < 0) { + point[i_n] = -point[i_n]; } - if (p_normal[i_n2]<0) { - point[i_n2]=-point[i_n2]; + if (p_normal[i_n2] < 0) { + point[i_n2] = -point[i_n2]; } r_supports[0] = point; - point[i]=-point[i]; + point[i] = -point[i]; r_supports[1] = point; return; } @@ -401,44 +374,38 @@ void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_suppo /* 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 - ); + (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_supports[0]=point; + r_amount = 1; + r_supports[0] = point; } -bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { - - Rect3 aabb(-half_extents,half_extents*2.0); +bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal); + Rect3 aabb(-half_extents, half_extents * 2.0); + return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } Vector3 BoxShapeSW::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) ); + 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 BoxShapeSW::_setup(const Vector3& p_half_extents) { - - half_extents=p_half_extents.abs(); - - configure(Rect3(-half_extents,half_extents*2)); +void BoxShapeSW::_setup(const Vector3 &p_half_extents) { + half_extents = p_half_extents.abs(); + configure(Rect3(-half_extents, half_extents * 2)); } -void BoxShapeSW::set_data(const Variant& p_data) { - +void BoxShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -448,138 +415,128 @@ Variant BoxShapeSW::get_data() const { return half_extents; } -BoxShapeSW::BoxShapeSW() { - - +BoxShapeSW::BoxShapeSW() { } - /********** CAPSULE *************/ +void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { - - Vector3 n=p_transform.basis.xform_inv(p_normal).normalized(); + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); real_t h = (n.z > 0) ? height : -height; n *= radius; n.z += h * 0.5; - r_max=p_normal.dot(p_transform.xform(n)); - r_min=p_normal.dot(p_transform.xform(-n)); + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); return; n = p_transform.basis.xform(n); - real_t distance = p_normal.dot( p_transform.origin ); + real_t distance = p_normal.dot(p_transform.origin); real_t length = Math::abs(p_normal.dot(n)); r_min = distance - length; r_max = distance + length; - ERR_FAIL_COND( r_max < r_min ); - + ERR_FAIL_COND(r_max < r_min); } -Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const { +Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 n=p_normal; + Vector3 n = p_normal; real_t h = (n.z > 0) ? height : -height; - n*=radius; - n.z += h*0.5; + n *= radius; + n.z += h * 0.5; return n; } -void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { - +void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n=p_normal; + Vector3 n = p_normal; real_t d = n.z; - if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { // make it flat - n.z=0.0; + n.z = 0.0; n.normalize(); - n*=radius; + n *= radius; - r_amount=2; - r_supports[0]=n; - r_supports[0].z+=height*0.5; - r_supports[1]=n; - r_supports[1].z-=height*0.5; + r_amount = 2; + r_supports[0] = n; + r_supports[0].z += height * 0.5; + r_supports[1] = n; + r_supports[1].z -= height * 0.5; } else { real_t h = (d > 0) ? height : -height; - n*=radius; - n.z += h*0.5; - r_amount=1; - *r_supports=n; - + n *= radius; + n.z += h * 0.5; + r_amount = 1; + *r_supports = n; } - } +bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { -bool CapsuleShapeSW::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 norm=(p_end-p_begin).normalized(); - real_t min_d=1e20; + Vector3 res, n; + bool collision = false; - - Vector3 res,n; - bool collision=false; - - Vector3 auxres,auxn; + Vector3 auxres, auxn; bool collided; // test against cylinder and spheres :-| - collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn); + collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } - collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn); + collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } - collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn); + collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn); if (collided) { - real_t d=norm.dot(auxres); + real_t d = norm.dot(auxres); - if (d<min_d) { - min_d=d; - res=auxres; - n=auxn; - collision=true; + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; } } if (collision) { - r_result=res; - r_normal=n; + r_result = res; + r_normal = n; } return collision; } @@ -587,105 +544,90 @@ bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_e Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + 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.y*extents.y + extents.y*extents.y) - ); - + (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.y * extents.y + extents.y * extents.y)); } +void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) { - - -void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius) { - - height=p_height; - radius=p_radius; - configure(Rect3(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0))); - + height = p_height; + radius = p_radius; + configure(Rect3(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); } -void CapsuleShapeSW::set_data(const Variant& p_data) { +void CapsuleShapeSW::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"]); - + _setup(d["height"], d["radius"]); } Variant CapsuleShapeSW::get_data() const { Dictionary d; - d["radius"]=radius; - d["height"]=height; + d["radius"] = radius; + d["height"] = height; return d; - } +CapsuleShapeSW::CapsuleShapeSW() { -CapsuleShapeSW::CapsuleShapeSW() { - - height=radius=0; - + height = radius = 0; } /********** CONVEX POLYGON *************/ +void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { - - - int vertex_count=mesh.vertices.size(); - if (vertex_count==0) + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) return; - const Vector3 *vrts=&mesh.vertices[0]; + const Vector3 *vrts = &mesh.vertices[0]; - for (int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { - real_t d=p_normal.dot( p_transform.xform( vrts[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; + if (i == 0 || d > r_max) + r_max = d; + if (i == 0 || d < r_min) + r_min = d; } } -Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const { +Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const { - Vector3 n=p_normal; + Vector3 n = p_normal; - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - int vertex_count=mesh.vertices.size(); - if (vertex_count==0) + int vertex_count = mesh.vertices.size(); + if (vertex_count == 0) return Vector3(); - const Vector3 *vrts=&mesh.vertices[0]; + const Vector3 *vrts = &mesh.vertices[0]; - for (int i=0;i<vertex_count;i++) { + for (int i = 0; i < vertex_count; i++) { - real_t d=n.dot(vrts[i]); + real_t d = n.dot(vrts[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } - return vrts[vert_support_idx]; - + return vrts[vert_support_idx]; } - - -void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -700,28 +642,27 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector real_t max; int vtx; - for (int i=0;i<vc;i++) { + for (int i = 0; i < vc; i++) { - real_t d=p_normal.dot(vertices[i]); + real_t d = p_normal.dot(vertices[i]); - if (i==0 || d > max) { - max=d; - vtx=i; + if (i == 0 || d > max) { + max = d; + vtx = i; } } + for (int i = 0; i < fc; i++) { - for(int i=0;i<fc;i++) { - - if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) { + if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { int ic = faces[i].indices.size(); - const int *ind=faces[i].indices.ptr(); + const int *ind = faces[i].indices.ptr(); - bool valid=false; - for(int j=0;j<ic;j++) { - if (ind[j]==vtx) { - valid=true; + bool valid = false; + for (int j = 0; j < ic; j++) { + if (ind[j] == vtx) { + valid = true; break; } } @@ -729,114 +670,103 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector if (!valid) continue; - int m = MIN(p_max,ic); - for(int j=0;j<m;j++) { + int m = MIN(p_max, ic); + for (int j = 0; j < m; j++) { - r_supports[j]=vertices[ind[j]]; + r_supports[j] = vertices[ind[j]]; } - r_amount=m; + r_amount = m; return; } } - for(int i=0;i<ec;i++) { + 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_TRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { - real_t dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal); - dot=ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) { - - r_amount=2; - r_supports[0]=vertices[edges[i].a]; - r_supports[1]=vertices[edges[i].b]; + r_amount = 2; + r_supports[0] = vertices[edges[i].a]; + r_supports[1] = vertices[edges[i].b]; return; } } - - r_supports[0]=vertices[vtx]; - r_amount=1; + r_supports[0] = vertices[vtx]; + r_amount = 1; } -bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { - - +bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); const Vector3 *vertices = mesh.vertices.ptr(); - Vector3 n = p_end-p_begin; + Vector3 n = p_end - p_begin; real_t min = 1e20; - bool col=false; + bool col = false; - for(int i=0;i<fc;i++) { + 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(); + const int *ind = faces[i].indices.ptr(); - for(int j=1;j<ic-1;j++) { + for (int j = 1; j < ic - 1; j++) { - Face3 f(vertices[ind[0]],vertices[ind[j]],vertices[ind[j+1]]); + Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); Vector3 result; - if (f.intersects_segment(p_begin,p_end,&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; + if (d < min) { + min = d; + r_result = result; + r_normal = faces[i].plane.normal; + col = true; } break; } - } } return col; - } Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy 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.y*extents.y + extents.y*extents.y) - ); + 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.y * extents.y + extents.y * extents.y)); } -void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices) { +void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) { - Error err = QuickHull::build(p_vertices,mesh); + Error err = QuickHull::build(p_vertices, mesh); Rect3 _aabb; - for(int i=0;i<mesh.vertices.size();i++) { + for (int i = 0; i < mesh.vertices.size(); i++) { - if (i==0) - _aabb.pos=mesh.vertices[i]; + if (i == 0) + _aabb.pos = mesh.vertices[i]; else _aabb.expand_to(mesh.vertices[i]); } configure(_aabb); - - } -void ConvexPolygonShapeSW::set_data(const Variant& p_data) { +void ConvexPolygonShapeSW::set_data(const Variant &p_data) { _setup(p_data); - } Variant ConvexPolygonShapeSW::get_data() const { @@ -844,113 +774,105 @@ Variant ConvexPolygonShapeSW::get_data() const { return mesh.vertices; } - -ConvexPolygonShapeSW::ConvexPolygonShapeSW() { - - +ConvexPolygonShapeSW::ConvexPolygonShapeSW() { } - /********** FACE POLYGON *************/ +void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { -void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { + for (int i = 0; i < 3; i++) { - for (int i=0;i<3;i++) { + Vector3 v = p_transform.xform(vertex[i]); + real_t d = p_normal.dot(v); - 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_max) - r_max=d; - - if (i==0 || d < r_min) - r_min=d; + if (i == 0 || d < r_min) + r_min = d; } } -Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const { - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - real_t d=p_normal.dot(vertex[i]); + real_t d = p_normal.dot(vertex[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } return vertex[vert_support_idx]; } -void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { +void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n=p_normal; + Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { - r_amount=3; - for (int i=0;i<3;i++) { + r_amount = 3; + for (int i = 0; i < 3; i++) { - r_supports[i]=vertex[i]; + r_supports[i] = vertex[i]; } return; - } /** FIND SUPPORT VERTEX **/ - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<3;i++) { + for (int i = 0; i < 3; i++) { - real_t d=n.dot(vertex[i]); + real_t d = n.dot(vertex[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=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++) { + for (int i = 0; i < 3; i++) { - int nx=(i+1)%3; - if (i!=vert_support_idx && nx!=vert_support_idx) + 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); + // 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_TRESHOLD) { - r_amount=2; - r_supports[0]=vertex[i]; - r_supports[1]=vertex[nx]; + r_amount = 2; + r_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; return; } } - r_amount=1; - r_supports[0]=vertex[vert_support_idx]; + r_amount = 1; + r_supports[0] = vertex[vert_support_idx]; } -bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - - bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result); + bool c = Geometry::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) { - r_normal=-r_normal; + r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; + if (r_normal.dot(p_end - p_begin) > 0) { + r_normal = -r_normal; } } @@ -960,183 +882,162 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end, Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! - } -FaceShapeSW::FaceShapeSW() { +FaceShapeSW::FaceShapeSW() { configure(Rect3()); - } - - PoolVector<Vector3> ConcavePolygonShapeSW::get_faces() const { - PoolVector<Vector3> rfaces; - rfaces.resize(faces.size()*3); + rfaces.resize(faces.size() * 3); - for(int i=0;i<faces.size();i++) { + for (int i = 0; i < faces.size(); i++) { - Face f=faces.get(i); + Face f = faces.get(i); - for(int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - rfaces.set(i*3+j, vertices.get( f.indices[j] ) ); + rfaces.set(i * 3 + j, vertices.get(f.indices[j])); } } return rfaces; } -void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - int count=vertices.size(); - if (count==0) { - r_min=0; - r_max=0; + int count = vertices.size(); + if (count == 0) { + r_min = 0; + r_max = 0; return; } - PoolVector<Vector3>::Read r=vertices.read(); - const Vector3 *vptr=r.ptr(); + PoolVector<Vector3>::Read r = vertices.read(); + const Vector3 *vptr = r.ptr(); - for (int i=0;i<count;i++) { + 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; + 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 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const { - int count=vertices.size(); - if (count==0) + int count = vertices.size(); + if (count == 0) return Vector3(); - PoolVector<Vector3>::Read r=vertices.read(); - const Vector3 *vptr=r.ptr(); + PoolVector<Vector3>::Read r = vertices.read(); + const Vector3 *vptr = r.ptr(); - Vector3 n=p_normal; + Vector3 n = p_normal; - int vert_support_idx=-1; + int vert_support_idx = -1; real_t support_max; - for (int i=0;i<count;i++) { + for (int i = 0; i < count; i++) { - real_t d=n.dot(vptr[i]); + real_t d = n.dot(vptr[i]); - if (i==0 || d > support_max) { - support_max=d; - vert_support_idx=i; + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; } } - return vptr[vert_support_idx]; - } -void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const { - - const BVH *bvh=&p_params->bvh[p_idx]; +void ConcavePolygonShapeSW::_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)) { + if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { return; } - - if (bvh->face_index>=0) { - + if (bvh->face_index >= 0) { Vector3 res; - Vector3 vertices[3]={ - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ], - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ], - p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ] + Vector3 vertices[3] = { + p_params->vertices[p_params->faces[bvh->face_index].indices[0]], + p_params->vertices[p_params->faces[bvh->face_index].indices[1]], + p_params->vertices[p_params->faces[bvh->face_index].indices[2]] }; if (Geometry::segment_intersects_triangle( - p_params->from, - p_params->to, - vertices[0], - vertices[1], - vertices[2], - &res)) { - - - real_t d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + p_params->from, + p_params->to, + vertices[0], + vertices[1], + vertices[2], + &res)) { + + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); //TODO, seems segmen/triangle intersection is broken :( - if (d>0 && d<p_params->min_d) { + if (d > 0 && d < p_params->min_d) { - p_params->min_d=d; - p_params->result=res; - p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal; - if (p_params->normal.dot(p_params->dir)>0) - p_params->normal=-p_params->normal; + p_params->min_d = d; + p_params->result = res; + p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal; + if (p_params->normal.dot(p_params->dir) > 0) + p_params->normal = -p_params->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); - - + if (bvh->left >= 0) + _cull_segment(bvh->left, p_params); + if (bvh->right >= 0) + _cull_segment(bvh->right, p_params); } } -bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { +bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - if (faces.size()==0) + if (faces.size() == 0) return false; // unlock data - PoolVector<Face>::Read fr=faces.read(); - PoolVector<Vector3>::Read vr=vertices.read(); - PoolVector<BVH>::Read br=bvh.read(); - + PoolVector<Face>::Read fr = faces.read(); + PoolVector<Vector3>::Read vr = vertices.read(); + PoolVector<BVH>::Read br = bvh.read(); _SegmentCullParams params; - params.from=p_begin; - params.to=p_end; - params.collisions=0; - params.dir=(p_end-p_begin).normalized(); + params.from = p_begin; + params.to = p_end; + params.collisions = 0; + params.dir = (p_end - p_begin).normalized(); - params.faces=fr.ptr(); - params.vertices=vr.ptr(); - params.bvh=br.ptr(); + params.faces = fr.ptr(); + params.vertices = vr.ptr(); + params.bvh = br.ptr(); - params.min_d=1e20; + params.min_d = 1e20; // cull - _cull_segment(0,¶ms); - - if (params.collisions>0) { + _cull_segment(0, ¶ms); + if (params.collisions > 0) { - r_result=params.result; - r_normal=params.normal; + r_result = params.result; + r_normal = params.normal; return true; } else { @@ -1144,81 +1045,76 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vecto } } -void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const { +void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { - const BVH* bvh=&p_params->bvh[p_idx]; + const BVH *bvh = &p_params->bvh[p_idx]; - if (!p_params->aabb.intersects( bvh->aabb )) + if (!p_params->aabb.intersects(bvh->aabb)) return; - if (bvh->face_index>=0) { + if (bvh->face_index >= 0) { - const Face *f=&p_params->faces[ bvh->face_index ]; - FaceShapeSW *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]]; - p_params->callback(p_params->userdata,face); + const Face *f = &p_params->faces[bvh->face_index]; + FaceShapeSW *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]]; + p_params->callback(p_params->userdata, face); } else { - if (bvh->left>=0) { - - _cull(bvh->left,p_params); + if (bvh->left >= 0) { + _cull(bvh->left, p_params); } - if (bvh->right>=0) { + if (bvh->right >= 0) { - _cull(bvh->right,p_params); + _cull(bvh->right, p_params); } - } } -void ConcavePolygonShapeSW::cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const { +void ConcavePolygonShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { // make matrix local to concave - if (faces.size()==0) + if (faces.size() == 0) return; - Rect3 local_aabb=p_local_aabb; + Rect3 local_aabb = p_local_aabb; // unlock data - PoolVector<Face>::Read fr=faces.read(); - PoolVector<Vector3>::Read vr=vertices.read(); - PoolVector<BVH>::Read br=bvh.read(); + PoolVector<Face>::Read fr = faces.read(); + PoolVector<Vector3>::Read vr = vertices.read(); + PoolVector<BVH>::Read br = bvh.read(); FaceShapeSW face; // use this to send in the callback _CullParams params; - params.aabb=local_aabb; - params.face=&face; - params.faces=fr.ptr(); - params.vertices=vr.ptr(); - params.bvh=br.ptr(); - params.callback=p_callback; - params.userdata=p_userdata; + params.aabb = local_aabb; + params.face = &face; + params.faces = fr.ptr(); + params.vertices = vr.ptr(); + params.bvh = br.ptr(); + params.callback = p_callback; + params.userdata = p_userdata; // cull - _cull(0,¶ms); - + _cull(0, ¶ms); } Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + 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.y*extents.y + extents.y*extents.y) - ); + (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.y * extents.y + extents.y * extents.y)); } - struct _VolumeSW_BVH_Element { Rect3 aabb; @@ -1228,26 +1124,25 @@ struct _VolumeSW_BVH_Element { struct _VolumeSW_BVH_CompareX { - _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.x<b.center.x; + 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 { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.y<b.center.y; + 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 { + _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.z<b.center.z; + return a.center.z < b.center.z; } }; @@ -1260,107 +1155,102 @@ struct _VolumeSW_BVH { int face_index; }; +_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { -_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) { - - _VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH ); + _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH); - if (p_size==1) { + if (p_size == 1) { //leaf - bvh->aabb=p_elements[0].aabb; - bvh->left=NULL; - bvh->right=NULL; - bvh->face_index=p_elements->face_index; + bvh->aabb = p_elements[0].aabb; + bvh->left = NULL; + bvh->right = NULL; + bvh->face_index = p_elements->face_index; count++; return bvh; } else { - bvh->face_index=-1; + bvh->face_index = -1; } Rect3 aabb; - for(int i=0;i<p_size;i++) { + for (int i = 0; i < p_size; i++) { - if (i==0) - aabb=p_elements[i].aabb; + 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()) { + 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); + 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); + 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); + 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); + 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 ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { -void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) { - - int idx=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; + 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); + 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; + 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); + 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; + p_bvh_array[p_idx].right = -1; } memdelete(p_bvh_tree); - } void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { - int src_face_count=p_faces.size(); - if (src_face_count==0) { + int src_face_count = p_faces.size(); + if (src_face_count == 0) { configure(Rect3()); return; } - ERR_FAIL_COND(src_face_count%3); - src_face_count/=3; + ERR_FAIL_COND(src_face_count % 3); + src_face_count /= 3; PoolVector<Vector3>::Read r = p_faces.read(); - const Vector3 * facesr= r.ptr(); + const Vector3 *facesr = r.ptr(); #if 0 Map<Vector3,int> point_map; @@ -1476,67 +1366,62 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { #else PoolVector<_VolumeSW_BVH_Element> bvh_array; - bvh_array.resize( src_face_count ); + bvh_array.resize(src_face_count); PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write(); - _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr(); + _VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr(); faces.resize(src_face_count); PoolVector<Face>::Write w = faces.write(); - Face *facesw=w.ptr(); + Face *facesw = w.ptr(); - vertices.resize( src_face_count*3 ); + vertices.resize(src_face_count * 3); PoolVector<Vector3>::Write vw = vertices.write(); - Vector3 *verticesw=vw.ptr(); + Vector3 *verticesw = vw.ptr(); Rect3 _aabb; + for (int i = 0; i < src_face_count; i++) { - for(int i=0;i<src_face_count;i++) { - - Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] ); + Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); - bvh_arrayw[i].aabb=face.get_aabb(); + bvh_arrayw[i].aabb = face.get_aabb(); bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5; - 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; + 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); - } - w=PoolVector<Face>::Write(); - vw=PoolVector<Vector3>::Write(); + w = PoolVector<Face>::Write(); + vw = PoolVector<Vector3>::Write(); - int count=0; - _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count); + int count = 0; + _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count); - bvh.resize( count+1 ); + bvh.resize(count + 1); PoolVector<BVH>::Write bvhw2 = bvh.write(); - BVH*bvh_arrayw2=bvhw2.ptr(); + BVH *bvh_arrayw2 = bvhw2.ptr(); - int idx=0; - _fill_bvh(bvh_tree,bvh_arrayw2,idx); + int idx = 0; + _fill_bvh(bvh_tree, bvh_arrayw2, idx); configure(_aabb); // this type of shape has no margin - #endif } - -void ConcavePolygonShapeSW::set_data(const Variant& p_data) { - +void ConcavePolygonShapeSW::set_data(const Variant &p_data) { _setup(p_data); } @@ -1547,12 +1432,8 @@ Variant ConcavePolygonShapeSW::get_data() const { } ConcavePolygonShapeSW::ConcavePolygonShapeSW() { - - } - - /* HEIGHT MAP SHAPE */ PoolVector<real_t> HeightMapShapeSW::get_heights() const { @@ -1572,114 +1453,94 @@ real_t HeightMapShapeSW::get_cell_size() const { return cell_size; } - -void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const { +void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &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 ); - + p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); } -Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const { - +Vector3 HeightMapShapeSW::get_support(const Vector3 &p_normal) const { //not very useful, but not very used either return get_aabb().get_support(p_normal); - } -bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const { - +bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { return false; } - -void HeightMapShapeSW::cull(const Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const { - - - +void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { } - Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const { - // use crappy AABB approximation - Vector3 extents=get_aabb().size*0.5; + 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.y*extents.y + extents.y*extents.y) - ); + (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.y * extents.y + extents.y * extents.y)); } +void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { -void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) { + heights = p_heights; + width = p_width; + depth = p_depth; + cell_size = p_cell_size; - heights=p_heights; - width=p_width; - depth=p_depth; - cell_size=p_cell_size; - - PoolVector<real_t>::Read r = heights. read(); + PoolVector<real_t>::Read r = heights.read(); Rect3 aabb; - for(int i=0;i<depth;i++) { + for (int i = 0; i < depth; i++) { - for(int j=0;j<width;j++) { + for (int j = 0; j < width; j++) { - real_t h = r[i*width+j]; + real_t h = r[i * width + j]; - Vector3 pos( j*cell_size, h, i*cell_size ); - if (i==0 || j==0) - aabb.pos=pos; + Vector3 pos(j * cell_size, h, i * cell_size); + if (i == 0 || j == 0) + aabb.pos = pos; else aabb.expand_to(pos); - } } - configure(aabb); } -void HeightMapShapeSW::set_data(const Variant& p_data) { +void HeightMapShapeSW::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("cell_size") ); - ERR_FAIL_COND( !d.has("heights") ); - - int width=d["width"]; - int depth=d["depth"]; - real_t cell_size=d["cell_size"]; - PoolVector<real_t> heights=d["heights"]; + 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("cell_size")); + ERR_FAIL_COND(!d.has("heights")); - ERR_FAIL_COND( width<= 0); - ERR_FAIL_COND( depth<= 0); - ERR_FAIL_COND( cell_size<= CMP_EPSILON); - ERR_FAIL_COND( heights.size() != (width*depth) ); - _setup(heights, width, depth, cell_size ); + int width = d["width"]; + int depth = d["depth"]; + real_t cell_size = d["cell_size"]; + PoolVector<real_t> heights = d["heights"]; + ERR_FAIL_COND(width <= 0); + ERR_FAIL_COND(depth <= 0); + ERR_FAIL_COND(cell_size <= CMP_EPSILON); + ERR_FAIL_COND(heights.size() != (width * depth)); + _setup(heights, width, depth, cell_size); } Variant HeightMapShapeSW::get_data() const { ERR_FAIL_V(Variant()); - } HeightMapShapeSW::HeightMapShapeSW() { - width=0; - depth=0; - cell_size=0; + width = 0; + depth = 0; + cell_size = 0; } - - - |