summaryrefslogtreecommitdiff
path: root/servers/physics
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics')
-rw-r--r--servers/physics/area_pair_sw.cpp3
-rw-r--r--servers/physics/area_sw.cpp6
-rw-r--r--servers/physics/area_sw.h3
-rw-r--r--servers/physics/body_pair_sw.cpp72
-rw-r--r--servers/physics/body_pair_sw.h1
-rw-r--r--servers/physics/body_sw.cpp16
-rw-r--r--servers/physics/body_sw.h3
-rw-r--r--servers/physics/collision_object_sw.cpp1
-rw-r--r--servers/physics/collision_object_sw.h11
-rw-r--r--servers/physics/collision_solver_sw.cpp44
-rw-r--r--servers/physics/collision_solver_sw.h1
-rw-r--r--servers/physics/constraint_sw.h6
-rw-r--r--servers/physics/physics_server_sw.cpp40
-rw-r--r--servers/physics/physics_server_sw.h6
-rw-r--r--servers/physics/space_sw.cpp5
-rw-r--r--servers/physics/step_sw.cpp39
16 files changed, 231 insertions, 26 deletions
diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index 4a303d3fdd..5938e716e9 100644
--- a/servers/physics/area_pair_sw.cpp
+++ b/servers/physics/area_pair_sw.cpp
@@ -67,7 +67,6 @@ void AreaPairSW::solve(float p_step) {
AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) {
-
body=p_body;
area=p_area;
body_shape=p_body_shape;
@@ -75,6 +74,8 @@ AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_are
colliding=false;
body->add_constraint(this,0);
area->add_constraint(this);
+ if (p_body->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC)
+ p_body->set_active(true);
}
diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp
index 8192a98400..4c9ac814be 100644
--- a/servers/physics/area_sw.cpp
+++ b/servers/physics/area_sw.cpp
@@ -148,8 +148,6 @@ void AreaSW::call_queries() {
return;
}
-
-
for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
if (E->get().state==0)
@@ -182,8 +180,8 @@ AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), move
point_attenuation=1;
density=0.1;
priority=0;
- ray_pickable=false;
-
+ set_ray_pickable(false);
+ monitor_callback_id=0;
}
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index a864055d17..c0c6e5c2ae 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -48,7 +48,6 @@ class AreaSW : public CollisionObjectSW{
float point_attenuation;
float density;
int priority;
- bool ray_pickable;
ObjectID monitor_callback_id;
StringName monitor_callback_method;
@@ -139,8 +138,6 @@ public:
_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
- _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; }
- _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index d112afa8e2..5847b942fb 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -172,10 +172,57 @@ void BodyPairSW::validate_contacts() {
}
}
+
+bool BodyPairSW::_test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B) {
+
+
+
+ Vector3 motion = p_A->get_linear_velocity()*p_step;
+ real_t mlen = motion.length();
+ if (mlen<CMP_EPSILON)
+ return false;
+
+ Vector3 mnormal = motion / mlen;
+
+ real_t min,max;
+ p_A->get_shape(p_shape_A)->project_range(mnormal,p_xform_A,min,max);
+ bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+
+ if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
+ return false;
+ }
+
+ //cast a segment from support in motion normal, in the same direction of motion by motion length
+ //support is the worst case collision point, so real collision happened before
+ int a;
+ Vector3 s=p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
+ Vector3 from = p_xform_A.xform(s);
+ Vector3 to = from + motion;
+
+ Transform from_inv = p_xform_B.affine_inverse();
+
+ Vector3 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+ Vector3 local_to = from_inv.xform(to);
+
+ Vector3 rpos,rnorm;
+ if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) {
+ return false;
+ }
+
+ //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
+ Vector3 hitpos = p_xform_B.xform(rpos);
+
+ float newlen = hitpos.distance_to(from)-(max-min)*0.01;
+ p_A->set_linear_velocity((mnormal*newlen)/p_step);
+
+ return true;
+}
+
+
bool BodyPairSW::setup(float p_step) {
//cannot collide
- if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
+ if ((A->get_layer_mask()&B->get_layer_mask())==0 || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
collided=false;
return false;
}
@@ -198,8 +245,21 @@ bool BodyPairSW::setup(float p_step) {
bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis);
this->collided=collided;
- if (!collided)
+
+ if (!collided) {
+
+ //test ccd (currently just a raycast)
+
+ if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
+ _test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B);
+ }
+
+ if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
+ _test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A);
+ }
+
return false;
+ }
@@ -267,6 +327,14 @@ bool BodyPairSW::setup(float p_step) {
B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA);
}
+ if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
+ c.active=false;
+ collided=false;
+ continue;
+
+ }
+
+
c.active=true;
// Precompute normal mass, tangent mass, and bias.
diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h
index 937c295c63..e64464e2c1 100644
--- a/servers/physics/body_pair_sw.h
+++ b/servers/physics/body_pair_sw.h
@@ -82,6 +82,7 @@ class BodyPairSW : public ConstraintSW {
void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B);
void validate_contacts();
+ bool _test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B);
SpaceSW *space;
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 0fd754ba25..725a440b59 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -185,6 +185,7 @@ float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
+ PhysicsServer::BodyMode prev=mode;
mode=p_mode;
switch(p_mode) {
@@ -196,8 +197,13 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
_inv_mass=0;
_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
//set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
+ set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC && contacts.size());
linear_velocity=Vector3();
angular_velocity=Vector3();
+ if (mode==PhysicsServer::BODY_MODE_KINEMATIC && prev!=mode) {
+ first_time_kinematic=true;
+ }
+
} break;
case PhysicsServer::BODY_MODE_RIGID: {
@@ -237,6 +243,11 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
new_transform=p_variant;
//wakeup_neighbours();
set_active(true);
+ if (first_time_kinematic) {
+ _set_transform(p_variant);
+ _set_inv_transform(get_transform().affine_inverse());
+ first_time_kinematic=false;
+ }
} else if (mode==PhysicsServer::BODY_MODE_STATIC) {
_set_transform(p_variant);
@@ -460,8 +471,8 @@ void BodySW::integrate_velocities(real_t p_step) {
if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
_set_transform(new_transform,false);
- _set_inv_transform(new_transform.affine_inverse()); ;
- if (linear_velocity==Vector3() && angular_velocity==Vector3())
+ _set_inv_transform(new_transform.affine_inverse());
+ if (contacts.size()==0 && linear_velocity==Vector3() && angular_velocity==Vector3())
set_active(false); //stopped moving, deactivate
return;
@@ -668,6 +679,7 @@ BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_upda
island_step=0;
island_next=NULL;
island_list_next=NULL;
+ first_time_kinematic=false;
_set_static(false);
density=0;
contact_count=0;
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 6317186d5f..ee3c76e455 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -74,6 +74,7 @@ class BodySW : public CollisionObjectSW {
bool continuous_cd;
bool can_sleep;
+ bool first_time_kinematic;
void _update_inertia();
virtual void _shapes_changed();
Transform new_transform;
@@ -137,7 +138,7 @@ public:
_FORCE_INLINE_ void add_area(AreaSW *p_area) { areas.insert(AreaCMP(p_area)); }
_FORCE_INLINE_ void remove_area(AreaSW *p_area) { areas.erase(AreaCMP(p_area)); }
- _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+ _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==PhysicsServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index f34aa19cae..c8e1fc4022 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -217,4 +217,5 @@ CollisionObjectSW::CollisionObjectSW(Type p_type) {
space=NULL;
instance_id=0;
layer_mask=1;
+ ray_pickable=true;
}
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index e717cc257c..788292ad2a 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -59,6 +59,9 @@ private:
BroadPhaseSW::ID bpid;
AABB aabb_cache; //for rayqueries
ShapeSW *shape;
+ bool trigger;
+
+ Shape() { trigger=false; }
};
Vector<Shape> shapes;
@@ -94,6 +97,9 @@ protected:
virtual void _shapes_changed()=0;
void _set_space(SpaceSW *space);
+ bool ray_pickable;
+
+
CollisionObjectSW(Type p_type);
public:
@@ -119,6 +125,11 @@ public:
_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ SpaceSW* get_space() const { return space; }
+ _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; }
+ _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
+
+ _FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+ _FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
_FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; }
_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp
index 56f2784145..86e3b679f2 100644
--- a/servers/physics/collision_solver_sw.cpp
+++ b/servers/physics/collision_solver_sw.cpp
@@ -275,6 +275,44 @@ void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_c
}
+
+bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B) {
+
+ const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A);
+ if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE)
+ return false;
+ Plane p = p_transform_A.xform(plane->get_plane());
+
+ static const int max_supports = 16;
+ Vector3 supports[max_supports];
+ int support_count;
+
+ p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count);
+
+ bool collided=false;
+ Vector3 closest;
+ float closest_d;
+
+
+ for(int i=0;i<support_count;i++) {
+
+ supports[i] = p_transform_B.xform( supports[i] );
+ real_t d = p.distance_to(supports[i]);
+ if (i==0 || d<closest_d) {
+ closest=supports[i];
+ closest_d=d;
+ if (d<=0)
+ collided=true;
+ }
+
+ }
+
+ r_point_A=p.project(closest);
+ r_point_B=closest;
+
+ return collided;
+}
+
bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) {
if (p_shape_A->is_concave())
@@ -282,7 +320,11 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform&
if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) {
- return false; //unsupported
+ Vector3 a,b;
+ bool col = solve_distance_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,a,b);
+ r_point_A=b;
+ r_point_B=a;
+ return !col;
} else if (p_shape_B->is_concave()) {
diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h
index 430f057c7c..764c32926c 100644
--- a/servers/physics/collision_solver_sw.h
+++ b/servers/physics/collision_solver_sw.h
@@ -42,6 +42,7 @@ private:
static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A=0,float p_margin_B=0);
static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex);
+ static bool solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B);
public:
diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h
index 1be96422e1..5e79c4b54c 100644
--- a/servers/physics/constraint_sw.h
+++ b/servers/physics/constraint_sw.h
@@ -38,12 +38,13 @@ class ConstraintSW {
uint64_t island_step;
ConstraintSW *island_next;
ConstraintSW *island_list_next;
+ int priority;
RID self;
protected:
- ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
+ ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; priority=1; }
public:
_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
@@ -62,6 +63,9 @@ public:
_FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; }
_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+ _FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+ _FORCE_INLINE_ int get_priority() const { return priority; }
+
virtual bool setup(float p_step)=0;
virtual void solve(float p_step)=0;
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 043d2149fe..cfe5a73ce1 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -540,6 +540,8 @@ void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool
BodySW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count());
+ body->set_shape_as_trigger(p_shape_idx,p_enable);
}
@@ -547,10 +549,9 @@ bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx)
BodySW *body = body_owner.get(p_body);
ERR_FAIL_COND_V(!body,false);
+ ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false);
- // todo ?
-
- return false;
+ body->is_shape_set_as_trigger(p_shape_idx);
}
@@ -769,7 +770,7 @@ void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b)
BodySW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
- body->remove_exception(p_body);
+ body->remove_exception(p_body_b);
};
@@ -836,6 +837,22 @@ void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_r
}
+void PhysicsServerSW::body_set_ray_pickable(RID p_body,bool p_enable) {
+
+ BodySW *body = body_owner.get(p_body);
+ ERR_FAIL_COND(!body);
+ body->set_ray_pickable(p_enable);
+
+}
+
+bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const{
+
+ BodySW *body = body_owner.get(p_body);
+ ERR_FAIL_COND_V(!body,false);
+ return body->is_ray_pickable();
+
+}
+
/* JOINT API */
@@ -1000,6 +1017,21 @@ bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) co
return hinge_joint->get_flag(p_flag);
}
+void PhysicsServerSW::joint_set_solver_priority(RID p_joint,int p_priority) {
+
+ JointSW *joint = joint_owner.get(p_joint);
+ ERR_FAIL_COND(!joint);
+ joint->set_priority(p_priority);
+}
+
+int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const{
+
+ JointSW *joint = joint_owner.get(p_joint);
+ ERR_FAIL_COND_V(!joint,0);
+ return joint->get_priority();
+
+}
+
PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const {
JointSW *joint = joint_owner.get(p_joint);
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index 76e5aecf55..6609a78662 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -201,6 +201,9 @@ public:
virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+ virtual void body_set_ray_pickable(RID p_body,bool p_enable);
+ virtual bool body_is_ray_pickable(RID p_body) const;
+
/* JOINT API */
virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B);
@@ -244,6 +247,9 @@ public:
virtual JointType joint_get_type(RID p_joint) const;
+ virtual void joint_set_solver_priority(RID p_joint,int p_priority);
+ virtual int joint_get_solver_priority(RID p_joint) const;
+
#if 0
virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 68d6b464ab..4e8b60b86b 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -71,12 +71,13 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
real_t min_d=1e10;
+
for(int i=0;i<amount;i++) {
if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
continue;
- if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA && !(static_cast<AreaSW*>(space->intersection_query_results[i])->is_ray_pickable()))
+ if (!(static_cast<AreaSW*>(space->intersection_query_results[i])->is_ray_pickable()))
continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
@@ -97,6 +98,8 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
+
+
Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
shape_point=xform.xform(shape_point);
diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp
index 6d95804875..b7d06d207b 100644
--- a/servers/physics/step_sw.cpp
+++ b/servers/physics/step_sw.cpp
@@ -27,7 +27,7 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "step_sw.h"
-
+#include "joints_sw.h"
void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) {
@@ -68,14 +68,41 @@ void StepSW::_setup_island(ConstraintSW *p_island,float p_delta) {
void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,float p_delta){
- for(int i=0;i<p_iterations;i++) {
+ int at_priority=1;
- ConstraintSW *ci=p_island;
- while(ci) {
- ci->solve(p_delta);
- ci=ci->get_island_next();
+ while(p_island) {
+
+ for(int i=0;i<p_iterations;i++) {
+
+ ConstraintSW *ci=p_island;
+ while(ci) {
+ ci->solve(p_delta);
+ ci=ci->get_island_next();
+ }
+ }
+
+ at_priority++;
+
+ {
+ ConstraintSW *ci=p_island;
+ ConstraintSW *prev=NULL;
+ while(ci) {
+ if (ci->get_priority()<at_priority) {
+ if (prev) {
+ prev->set_island_next(ci->get_island_next()); //remove
+ } else {
+ p_island=ci->get_island_next();
+ }
+ } else {
+
+ prev=ci;
+ }
+
+ ci=ci->get_island_next();
+ }
}
}
+
}
void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-07 10:11:51 +0000