summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_2d_sw.cpp8
-rw-r--r--servers/physics_2d/collision_solver_2d_sat.cpp1
-rw-r--r--servers/physics_2d/shape_2d_sw.cpp33
-rw-r--r--servers/physics_2d/shape_2d_sw.h33
4 files changed, 45 insertions, 30 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index fbad19f6be..591bf046ef 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -65,7 +65,13 @@ void Body2DSW::update_inertias() {
float mass = area * this->mass / total_area;
- _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared();
+ Matrix32 mtx = get_shape_transform(i);
+ Vector2 scale = mtx.get_scale();
+ _inertia += shape->get_moment_of_inertia(mass,scale) + mass * mtx.get_origin().length_squared();
+ //Rect2 ab = get_shape_aabb(i);
+ //_inertia+=mass*ab.size.dot(ab.size)/12.0f;
+
+
}
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index f73ed5732e..9ed594e0eb 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -1086,6 +1086,7 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+
if (!separator.test_previous_axis())
return;
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 012c4ed404..336eec73b5 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -131,7 +131,7 @@ bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_en
return true;
}
-real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t LineShape2DSW::get_moment_of_inertia(float p_mass, const Vector2 &p_scale) const {
return 0;
}
@@ -180,7 +180,7 @@ bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end
}
-real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t RayShape2DSW::get_moment_of_inertia(float p_mass, const Vector2 &p_scale) const {
return 0; //rays are mass-less
}
@@ -237,10 +237,12 @@ bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p
return true;
}
-real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass, const Vector2 &p_scale) const {
- real_t l = b.distance_to(a);
- Vector2 ofs = (a+b)*0.5;
+ 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());
}
@@ -312,9 +314,10 @@ bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_
return true;
}
-real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {
+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);
- return radius*radius;
}
void CircleShape2DSW::set_data(const Variant& p_data) {
@@ -377,9 +380,9 @@ bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2&
return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
}
-real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass,const Vector2& p_scale) const {
- Vector2 he2=half_extents*2;
+ Vector2 he2=half_extents*2*p_scale;
return p_mass*he2.dot(he2)/12.0f;
}
@@ -499,9 +502,9 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p
return collided; //todo
}
-real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass, const Vector2 &p_scale) const {
- Vector2 he2(radius*2,height+radius*2);
+ Vector2 he2=Vector2(radius*2,height+radius*2)*p_scale;
return p_mass*he2.dot(he2)/12.0f;
}
@@ -610,16 +613,16 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vect
return inters; //todo
}
-real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {
+real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass,const Vector2& p_scale) const {
Rect2 aabb;
- aabb.pos=points[0].pos;
+ aabb.pos=points[0].pos*p_scale;
for(int i=0;i<point_count;i++) {
- aabb.expand_to(points[i].pos);
+ aabb.expand_to(points[i].pos*p_scale);
}
- return p_mass*aabb.size.dot(aabb.size)/12.0f;
+ 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) {
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index 8500a6194f..51ece9fc7e 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -84,7 +84,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;
- virtual real_t get_moment_of_inertia(float p_mass) const=0;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const=0;
virtual void set_data(const Variant& p_data)=0;
virtual Variant get_data() const=0;
@@ -173,7 +173,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -215,7 +215,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -262,7 +262,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -299,7 +299,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -338,20 +338,25 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
// no matter the angle, the box is mirrored anyway
- Vector2 local_normal=p_transform.basis_xform_inv(p_normal);
+ r_max=-1e20;
+ r_min=1e20;
+ for(int i=0;i<4;i++) {
- float length = local_normal.abs().dot(half_extents);
- float distance = p_normal.dot( p_transform.get_origin() );
+ real_t d=p_normal.dot(p_transform.xform(Vector2( ((i&1)*2-1)*half_extents.x, ((i>>1)*2-1)*half_extents.y )));
- r_min = distance - length;
- r_max = distance + length;
+ if (d>r_max)
+ r_max=d;
+ if (d<r_min)
+ r_min=d;
+
+ }
}
@@ -420,7 +425,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -482,7 +487,7 @@ public:
virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const;
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
@@ -570,7 +575,7 @@ public:
virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
- virtual real_t get_moment_of_inertia(float p_mass) const { return 0; }
+ virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const { return 0; }
virtual void set_data(const Variant& p_data);
virtual Variant get_data() const;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-30 03:17:59 +0000