You've already forked godot
mirror of
https://github.com/godotengine/godot.git
synced 2025-11-04 12:00:25 +00:00
-improved physics ccd
-html5 exporter works again -disable repeat on image loader by default -can change shape offset en tileset, texture offset was broken
This commit is contained in:
Juan Linietsky
@@ -198,7 +198,7 @@ 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_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) {
|
||||
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)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -84,7 +84,7 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
|
||||
|
||||
void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) {
|
||||
bool do_override=p_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
|
||||
if (do_override==space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
|
||||
if (do_override==(space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED))
|
||||
return;
|
||||
_unregister_shapes();
|
||||
space_override_mode=p_mode;
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
|
||||
bool AreaPair2DSW::setup(float p_step) {
|
||||
|
||||
bool result = CollisionSolver2DSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),body->get_shape_inv_transform(body_shape) * body->get_inv_transform(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),area->get_shape_inv_transform(area_shape) * area->get_inv_transform(),NULL,this);
|
||||
bool result = CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this);
|
||||
|
||||
if (result!=colliding) {
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -26,309 +26,321 @@
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
#ifndef BODY_2D_SW_H
|
||||
#define BODY_2D_SW_H
|
||||
|
||||
#include "collision_object_2d_sw.h"
|
||||
#include "vset.h"
|
||||
#include "area_2d_sw.h"
|
||||
|
||||
class Constraint2DSW;
|
||||
|
||||
|
||||
class Body2DSW : public CollisionObject2DSW {
|
||||
|
||||
|
||||
Physics2DServer::BodyMode mode;
|
||||
|
||||
Vector2 biased_linear_velocity;
|
||||
real_t biased_angular_velocity;
|
||||
|
||||
Vector2 linear_velocity;
|
||||
real_t angular_velocity;
|
||||
|
||||
real_t mass;
|
||||
real_t bounce;
|
||||
real_t friction;
|
||||
|
||||
real_t _inv_mass;
|
||||
real_t _inv_inertia;
|
||||
|
||||
Vector2 gravity;
|
||||
real_t density;
|
||||
|
||||
real_t still_time;
|
||||
|
||||
Vector2 applied_force;
|
||||
real_t applied_torque;
|
||||
|
||||
SelfList<Body2DSW> active_list;
|
||||
SelfList<Body2DSW> inertia_update_list;
|
||||
SelfList<Body2DSW> direct_state_query_list;
|
||||
|
||||
VSet<RID> exceptions;
|
||||
bool omit_force_integration;
|
||||
bool active;
|
||||
bool simulated_motion;
|
||||
bool continuous_cd;
|
||||
bool can_sleep;
|
||||
void _update_inertia();
|
||||
virtual void _shapes_changed();
|
||||
|
||||
|
||||
Map<Constraint2DSW*,int> constraint_map;
|
||||
|
||||
struct AreaCMP {
|
||||
|
||||
Area2DSW *area;
|
||||
_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
|
||||
_FORCE_INLINE_ AreaCMP() {}
|
||||
_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
|
||||
};
|
||||
|
||||
|
||||
VSet<AreaCMP> areas;
|
||||
|
||||
struct Contact {
|
||||
|
||||
|
||||
Vector2 local_pos;
|
||||
Vector2 local_normal;
|
||||
float depth;
|
||||
int local_shape;
|
||||
Vector2 collider_pos;
|
||||
int collider_shape;
|
||||
ObjectID collider_instance_id;
|
||||
RID collider;
|
||||
Vector2 collider_velocity_at_pos;
|
||||
};
|
||||
|
||||
Vector<Contact> contacts; //no contacts by default
|
||||
int contact_count;
|
||||
|
||||
struct ForceIntegrationCallback {
|
||||
|
||||
ObjectID id;
|
||||
StringName method;
|
||||
Variant callback_udata;
|
||||
};
|
||||
|
||||
ForceIntegrationCallback *fi_callback;
|
||||
|
||||
|
||||
uint64_t island_step;
|
||||
Body2DSW *island_next;
|
||||
Body2DSW *island_list_next;
|
||||
|
||||
_FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);
|
||||
|
||||
friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
|
||||
|
||||
public:
|
||||
|
||||
|
||||
void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
|
||||
|
||||
|
||||
_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
|
||||
_FORCE_INLINE_ void remove_area(Area2DSW *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_ int get_max_contacts_reported() const { return contacts.size(); }
|
||||
|
||||
_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
|
||||
_FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
|
||||
|
||||
|
||||
_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
|
||||
_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
|
||||
_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
|
||||
_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
|
||||
|
||||
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
|
||||
_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
|
||||
|
||||
_FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }
|
||||
_FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
|
||||
|
||||
_FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
|
||||
_FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
|
||||
|
||||
_FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
|
||||
_FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
|
||||
const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
|
||||
|
||||
_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
|
||||
_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
|
||||
|
||||
_FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {
|
||||
|
||||
linear_velocity += p_j * _inv_mass;
|
||||
angular_velocity += _inv_inertia * p_pos.cross(p_j);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
|
||||
|
||||
biased_linear_velocity += p_j * _inv_mass;
|
||||
biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
|
||||
}
|
||||
|
||||
void set_active(bool p_active);
|
||||
_FORCE_INLINE_ bool is_active() const { return active; }
|
||||
|
||||
void set_param(Physics2DServer::BodyParameter p_param, float);
|
||||
float get_param(Physics2DServer::BodyParameter p_param) const;
|
||||
|
||||
void set_mode(Physics2DServer::BodyMode p_mode);
|
||||
Physics2DServer::BodyMode get_mode() const;
|
||||
|
||||
void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
|
||||
Variant get_state(Physics2DServer::BodyState p_state) const;
|
||||
|
||||
void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
|
||||
Vector2 get_applied_force() const { return applied_force; }
|
||||
|
||||
void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
|
||||
real_t get_applied_torque() const { return applied_torque; }
|
||||
|
||||
_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
|
||||
_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
|
||||
|
||||
void set_space(Space2DSW *p_space);
|
||||
|
||||
void update_inertias();
|
||||
|
||||
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
|
||||
_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
|
||||
_FORCE_INLINE_ real_t get_friction() const { return friction; }
|
||||
_FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
|
||||
_FORCE_INLINE_ real_t get_density() const { return density; }
|
||||
|
||||
void integrate_forces(real_t p_step);
|
||||
void integrate_velocities(real_t p_step);
|
||||
|
||||
void simulate_motion(const Matrix32& p_xform,real_t p_step);
|
||||
void call_queries();
|
||||
void wakeup_neighbours();
|
||||
|
||||
bool sleep_test(real_t p_step);
|
||||
|
||||
Body2DSW();
|
||||
~Body2DSW();
|
||||
|
||||
};
|
||||
|
||||
|
||||
//add contact inline
|
||||
|
||||
void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
|
||||
|
||||
int c_max=contacts.size();
|
||||
|
||||
if (c_max==0)
|
||||
return;
|
||||
|
||||
Contact *c = &contacts[0];
|
||||
|
||||
|
||||
int idx=-1;
|
||||
|
||||
if (contact_count<c_max) {
|
||||
idx=contact_count++;
|
||||
} else {
|
||||
|
||||
float least_depth=1e20;
|
||||
int least_deep=-1;
|
||||
for(int i=0;i<c_max;i++) {
|
||||
|
||||
if (i==0 || c[i].depth<least_depth) {
|
||||
least_deep=i;
|
||||
least_depth=c[i].depth;
|
||||
}
|
||||
}
|
||||
|
||||
if (least_deep>=0 && least_depth<p_depth) {
|
||||
|
||||
idx=least_deep;
|
||||
}
|
||||
if (idx==-1)
|
||||
return; //none least deepe than this
|
||||
}
|
||||
|
||||
c[idx].local_pos=p_local_pos;
|
||||
c[idx].local_normal=p_local_normal;
|
||||
c[idx].depth=p_depth;
|
||||
c[idx].local_shape=p_local_shape;
|
||||
c[idx].collider_pos=p_collider_pos;
|
||||
c[idx].collider_shape=p_collider_shape;
|
||||
c[idx].collider_instance_id=p_collider_instance_id;
|
||||
c[idx].collider=p_collider;
|
||||
c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
|
||||
|
||||
}
|
||||
|
||||
|
||||
class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
|
||||
|
||||
OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
|
||||
|
||||
public:
|
||||
|
||||
static Physics2DDirectBodyStateSW *singleton;
|
||||
Body2DSW *body;
|
||||
real_t step;
|
||||
|
||||
virtual Vector2 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area
|
||||
virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area
|
||||
|
||||
virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
|
||||
virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
|
||||
|
||||
virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); }
|
||||
virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
|
||||
|
||||
virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
|
||||
virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
|
||||
|
||||
virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); }
|
||||
virtual Matrix32 get_transform() const { return body->get_transform(); }
|
||||
|
||||
virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
|
||||
virtual bool is_sleeping() const { return !body->is_active(); }
|
||||
|
||||
virtual int get_contact_count() const { return body->contact_count; }
|
||||
|
||||
virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
|
||||
ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
|
||||
return body->contacts[p_contact_idx].local_pos;
|
||||
}
|
||||
virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; }
|
||||
virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; }
|
||||
|
||||
virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; }
|
||||
virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; }
|
||||
virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; }
|
||||
virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; }
|
||||
virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; }
|
||||
|
||||
virtual Physics2DDirectSpaceState* get_space_state();
|
||||
|
||||
|
||||
virtual real_t get_step() const { return step; }
|
||||
Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
|
||||
};
|
||||
|
||||
|
||||
#endif // BODY_2D_SW_H
|
||||
#ifndef BODY_2D_SW_H
|
||||
#define BODY_2D_SW_H
|
||||
|
||||
#include "collision_object_2d_sw.h"
|
||||
#include "vset.h"
|
||||
#include "area_2d_sw.h"
|
||||
|
||||
class Constraint2DSW;
|
||||
|
||||
|
||||
class Body2DSW : public CollisionObject2DSW {
|
||||
|
||||
|
||||
Physics2DServer::BodyMode mode;
|
||||
|
||||
Vector2 biased_linear_velocity;
|
||||
real_t biased_angular_velocity;
|
||||
|
||||
Vector2 linear_velocity;
|
||||
real_t angular_velocity;
|
||||
|
||||
real_t mass;
|
||||
real_t bounce;
|
||||
real_t friction;
|
||||
|
||||
real_t _inv_mass;
|
||||
real_t _inv_inertia;
|
||||
|
||||
Vector2 gravity;
|
||||
real_t density;
|
||||
|
||||
real_t still_time;
|
||||
|
||||
Vector2 applied_force;
|
||||
real_t applied_torque;
|
||||
|
||||
SelfList<Body2DSW> active_list;
|
||||
SelfList<Body2DSW> inertia_update_list;
|
||||
SelfList<Body2DSW> direct_state_query_list;
|
||||
|
||||
VSet<RID> exceptions;
|
||||
Physics2DServer::CCDMode continuous_cd_mode;
|
||||
bool omit_force_integration;
|
||||
bool active;
|
||||
bool can_sleep;
|
||||
void _update_inertia();
|
||||
virtual void _shapes_changed();
|
||||
Matrix32 new_transform;
|
||||
|
||||
|
||||
Map<Constraint2DSW*,int> constraint_map;
|
||||
|
||||
struct AreaCMP {
|
||||
|
||||
Area2DSW *area;
|
||||
_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
|
||||
_FORCE_INLINE_ AreaCMP() {}
|
||||
_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
|
||||
};
|
||||
|
||||
|
||||
VSet<AreaCMP> areas;
|
||||
|
||||
struct Contact {
|
||||
|
||||
|
||||
Vector2 local_pos;
|
||||
Vector2 local_normal;
|
||||
float depth;
|
||||
int local_shape;
|
||||
Vector2 collider_pos;
|
||||
int collider_shape;
|
||||
ObjectID collider_instance_id;
|
||||
RID collider;
|
||||
Vector2 collider_velocity_at_pos;
|
||||
};
|
||||
|
||||
Vector<Contact> contacts; //no contacts by default
|
||||
int contact_count;
|
||||
|
||||
struct ForceIntegrationCallback {
|
||||
|
||||
ObjectID id;
|
||||
StringName method;
|
||||
Variant callback_udata;
|
||||
};
|
||||
|
||||
ForceIntegrationCallback *fi_callback;
|
||||
|
||||
|
||||
uint64_t island_step;
|
||||
Body2DSW *island_next;
|
||||
Body2DSW *island_list_next;
|
||||
|
||||
_FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);
|
||||
|
||||
friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
|
||||
|
||||
public:
|
||||
|
||||
|
||||
void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
|
||||
|
||||
|
||||
_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
|
||||
_FORCE_INLINE_ void remove_area(Area2DSW *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_ int get_max_contacts_reported() const { return contacts.size(); }
|
||||
|
||||
_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
|
||||
_FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
|
||||
|
||||
|
||||
_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
|
||||
_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
|
||||
_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
|
||||
_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
|
||||
|
||||
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
|
||||
_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
|
||||
|
||||
_FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }
|
||||
_FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
|
||||
|
||||
_FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
|
||||
_FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
|
||||
|
||||
_FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
|
||||
_FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
|
||||
const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
|
||||
|
||||
_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
|
||||
_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
|
||||
|
||||
_FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
|
||||
|
||||
_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
|
||||
_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
|
||||
|
||||
|
||||
_FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {
|
||||
|
||||
linear_velocity += p_j * _inv_mass;
|
||||
angular_velocity += _inv_inertia * p_pos.cross(p_j);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
|
||||
|
||||
biased_linear_velocity += p_j * _inv_mass;
|
||||
biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
|
||||
}
|
||||
|
||||
void set_active(bool p_active);
|
||||
_FORCE_INLINE_ bool is_active() const { return active; }
|
||||
|
||||
void set_param(Physics2DServer::BodyParameter p_param, float);
|
||||
float get_param(Physics2DServer::BodyParameter p_param) const;
|
||||
|
||||
void set_mode(Physics2DServer::BodyMode p_mode);
|
||||
Physics2DServer::BodyMode get_mode() const;
|
||||
|
||||
void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
|
||||
Variant get_state(Physics2DServer::BodyState p_state) const;
|
||||
|
||||
void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
|
||||
Vector2 get_applied_force() const { return applied_force; }
|
||||
|
||||
void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
|
||||
real_t get_applied_torque() const { return applied_torque; }
|
||||
|
||||
|
||||
_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; }
|
||||
_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
|
||||
|
||||
void set_space(Space2DSW *p_space);
|
||||
|
||||
void update_inertias();
|
||||
|
||||
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
|
||||
_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
|
||||
_FORCE_INLINE_ real_t get_friction() const { return friction; }
|
||||
_FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
|
||||
_FORCE_INLINE_ real_t get_density() const { return density; }
|
||||
_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
|
||||
|
||||
void integrate_forces(real_t p_step);
|
||||
void integrate_velocities(real_t p_step);
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_motion() const {
|
||||
|
||||
if (mode>Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
return new_transform.get_origin() - get_transform().get_origin();
|
||||
} else if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
return get_transform().get_origin() -new_transform.get_origin(); //kinematic simulates forward
|
||||
}
|
||||
return Vector2();
|
||||
}
|
||||
|
||||
void call_queries();
|
||||
void wakeup_neighbours();
|
||||
|
||||
bool sleep_test(real_t p_step);
|
||||
|
||||
Body2DSW();
|
||||
~Body2DSW();
|
||||
|
||||
};
|
||||
|
||||
|
||||
//add contact inline
|
||||
|
||||
void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
|
||||
|
||||
int c_max=contacts.size();
|
||||
|
||||
if (c_max==0)
|
||||
return;
|
||||
|
||||
Contact *c = &contacts[0];
|
||||
|
||||
|
||||
int idx=-1;
|
||||
|
||||
if (contact_count<c_max) {
|
||||
idx=contact_count++;
|
||||
} else {
|
||||
|
||||
float least_depth=1e20;
|
||||
int least_deep=-1;
|
||||
for(int i=0;i<c_max;i++) {
|
||||
|
||||
if (i==0 || c[i].depth<least_depth) {
|
||||
least_deep=i;
|
||||
least_depth=c[i].depth;
|
||||
}
|
||||
}
|
||||
|
||||
if (least_deep>=0 && least_depth<p_depth) {
|
||||
|
||||
idx=least_deep;
|
||||
}
|
||||
if (idx==-1)
|
||||
return; //none least deepe than this
|
||||
}
|
||||
|
||||
c[idx].local_pos=p_local_pos;
|
||||
c[idx].local_normal=p_local_normal;
|
||||
c[idx].depth=p_depth;
|
||||
c[idx].local_shape=p_local_shape;
|
||||
c[idx].collider_pos=p_collider_pos;
|
||||
c[idx].collider_shape=p_collider_shape;
|
||||
c[idx].collider_instance_id=p_collider_instance_id;
|
||||
c[idx].collider=p_collider;
|
||||
c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
|
||||
|
||||
}
|
||||
|
||||
|
||||
class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
|
||||
|
||||
OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
|
||||
|
||||
public:
|
||||
|
||||
static Physics2DDirectBodyStateSW *singleton;
|
||||
Body2DSW *body;
|
||||
real_t step;
|
||||
|
||||
virtual Vector2 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area
|
||||
virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area
|
||||
|
||||
virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
|
||||
virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
|
||||
|
||||
virtual void set_linear_velocity(const Vector2& p_velocity) { body->set_linear_velocity(p_velocity); }
|
||||
virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
|
||||
|
||||
virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
|
||||
virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
|
||||
|
||||
virtual void set_transform(const Matrix32& p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform); }
|
||||
virtual Matrix32 get_transform() const { return body->get_transform(); }
|
||||
|
||||
virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
|
||||
virtual bool is_sleeping() const { return !body->is_active(); }
|
||||
|
||||
virtual int get_contact_count() const { return body->contact_count; }
|
||||
|
||||
virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
|
||||
ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
|
||||
return body->contacts[p_contact_idx].local_pos;
|
||||
}
|
||||
virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].local_normal; }
|
||||
virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; }
|
||||
|
||||
virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; }
|
||||
virtual Vector2 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_pos; }
|
||||
virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; }
|
||||
virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; }
|
||||
virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos; }
|
||||
|
||||
virtual Physics2DDirectSpaceState* get_space_state();
|
||||
|
||||
|
||||
virtual real_t get_step() const { return step; }
|
||||
Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
|
||||
};
|
||||
|
||||
|
||||
#endif // BODY_2D_SW_H
|
||||
|
||||
@@ -46,7 +46,6 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
|
||||
|
||||
// check if we already have the contact
|
||||
|
||||
|
||||
Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
|
||||
Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B);
|
||||
|
||||
@@ -61,6 +60,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
|
||||
contact.acc_tangent_impulse=0;
|
||||
contact.local_A=local_A;
|
||||
contact.local_B=local_B;
|
||||
contact.reused=true;
|
||||
contact.normal=(p_point_A-p_point_B).normalized();
|
||||
|
||||
// attempt to determine if the contact will be reused
|
||||
@@ -77,7 +77,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
|
||||
contact.acc_normal_impulse=c.acc_normal_impulse;
|
||||
contact.acc_tangent_impulse=c.acc_tangent_impulse;
|
||||
contact.acc_bias_impulse=c.acc_bias_impulse;
|
||||
new_index=i;
|
||||
new_index=i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -139,12 +139,26 @@ void BodyPair2DSW::_validate_contacts() {
|
||||
|
||||
Contact& c = contacts[i];
|
||||
|
||||
Vector2 global_A = A->get_transform().basis_xform(c.local_A);
|
||||
Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
|
||||
Vector2 axis = global_A - global_B;
|
||||
float depth = axis.dot( c.normal );
|
||||
bool erase=false;
|
||||
if (c.reused==false) {
|
||||
//was left behind in previous frame
|
||||
erase=true;
|
||||
} else {
|
||||
c.reused=false;
|
||||
|
||||
if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
|
||||
Vector2 global_A = A->get_transform().basis_xform(c.local_A);
|
||||
Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
|
||||
Vector2 axis = global_A - global_B;
|
||||
float depth = axis.dot( c.normal );
|
||||
|
||||
|
||||
|
||||
if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
|
||||
erase=true;
|
||||
}
|
||||
}
|
||||
|
||||
if (erase) {
|
||||
// contact no longer needed, remove
|
||||
|
||||
|
||||
@@ -161,7 +175,9 @@ void BodyPair2DSW::_validate_contacts() {
|
||||
}
|
||||
|
||||
|
||||
bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result) {
|
||||
bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) {
|
||||
|
||||
|
||||
|
||||
Vector2 motion = p_A->get_linear_velocity()*p_step;
|
||||
real_t mlen = motion.length();
|
||||
@@ -172,18 +188,24 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
|
||||
|
||||
real_t min,max;
|
||||
p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
|
||||
if (mlen < (max-min)*0.3) //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
|
||||
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;
|
||||
Vector2 s[2];
|
||||
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
|
||||
Vector2 from = p_xform_A.xform(s[0]);
|
||||
Vector2 to = from + motion;
|
||||
|
||||
Vector2 local_from = p_xform_inv_B.xform(from);
|
||||
Vector2 local_to = p_xform_inv_B.xform(to);
|
||||
Matrix32 from_inv = p_xform_B.affine_inverse();
|
||||
|
||||
Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
|
||||
Vector2 local_to = from_inv.xform(to);
|
||||
|
||||
Vector2 rpos,rnorm;
|
||||
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
|
||||
@@ -191,8 +213,11 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
|
||||
|
||||
//ray hit something
|
||||
|
||||
|
||||
Vector2 hitpos = p_xform_B.xform(rpos);
|
||||
|
||||
Vector2 contact_A = to;
|
||||
Vector2 contact_B = p_xform_B.xform(rpos);
|
||||
Vector2 contact_B = hitpos;
|
||||
|
||||
//create a contact
|
||||
|
||||
@@ -208,41 +233,50 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
|
||||
bool BodyPair2DSW::setup(float p_step) {
|
||||
|
||||
|
||||
//cannot collide
|
||||
if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) {
|
||||
collided=false;
|
||||
return false;
|
||||
}
|
||||
|
||||
//use local A coordinates to avoid numerical issues on collision detection
|
||||
offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
|
||||
|
||||
_validate_contacts();
|
||||
|
||||
//cannot collide
|
||||
if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE)) {
|
||||
collided=false;
|
||||
|
||||
return false;
|
||||
}
|
||||
Vector2 offset_A = A->get_transform().get_origin();
|
||||
Matrix32 xform_Au = A->get_transform().untranslated();
|
||||
Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
|
||||
Matrix32 xform_inv_A = xform_A.affine_inverse();
|
||||
|
||||
Matrix32 xform_Bu = B->get_transform();
|
||||
xform_Bu.elements[2]-=A->get_transform().get_origin();
|
||||
Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
|
||||
Matrix32 xform_inv_B = xform_B.affine_inverse();
|
||||
|
||||
Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
|
||||
Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
|
||||
|
||||
collided = CollisionSolver2DSW::solve_static(shape_A_ptr,xform_A,xform_inv_A,shape_B_ptr,xform_B,xform_inv_B,_add_contact,this,&sep_axis);
|
||||
Vector2 motion_A,motion_B;
|
||||
|
||||
if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
|
||||
motion_A=A->get_motion();
|
||||
}
|
||||
if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
|
||||
motion_B=B->get_motion();
|
||||
}
|
||||
//faster to set than to check..
|
||||
|
||||
collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis);
|
||||
if (!collided) {
|
||||
|
||||
//test ccd (currently just a raycast)
|
||||
if (A->is_continuous_collision_detection_enabled() && A->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
|
||||
if (_test_ccd(p_step,A,shape_A,xform_A,xform_inv_A,B,shape_B,xform_B,xform_inv_B))
|
||||
|
||||
if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B))
|
||||
collided=true;
|
||||
}
|
||||
|
||||
if (B->is_continuous_collision_detection_enabled() && B->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
|
||||
if (_test_ccd(p_step,B,shape_B,xform_B,xform_inv_B,A,shape_A,xform_A,xform_inv_A,true))
|
||||
if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true))
|
||||
collided=true;
|
||||
}
|
||||
|
||||
@@ -251,8 +285,6 @@ bool BodyPair2DSW::setup(float p_step) {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
real_t max_penetration = space->get_contact_max_allowed_penetration();
|
||||
|
||||
float bias = 0.3f;
|
||||
@@ -280,7 +312,7 @@ bool BodyPair2DSW::setup(float p_step) {
|
||||
|
||||
real_t depth = c.normal.dot(global_A - global_B);
|
||||
|
||||
if (depth<=0) {
|
||||
if (depth<=0 || !c.reused) {
|
||||
c.active=false;
|
||||
continue;
|
||||
}
|
||||
@@ -311,7 +343,6 @@ bool BodyPair2DSW::setup(float p_step) {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Precompute normal mass, tangent mass, and bias.
|
||||
real_t rnA = c.rA.dot(c.normal);
|
||||
real_t rnB = c.rB.dot(c.normal);
|
||||
@@ -373,6 +404,7 @@ void BodyPair2DSW::solve(float p_step) {
|
||||
Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x );
|
||||
Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
|
||||
|
||||
|
||||
real_t vn = dv.dot(c.normal);
|
||||
real_t vbn = dbv.dot(c.normal);
|
||||
Vector2 tangent = c.normal.tangent();
|
||||
@@ -388,7 +420,7 @@ void BodyPair2DSW::solve(float p_step) {
|
||||
A->apply_bias_impulse(c.rA,-jb);
|
||||
B->apply_bias_impulse(c.rB, jb);
|
||||
|
||||
real_t bounce=0;
|
||||
real_t bounce=MAX(A->get_bounce(),B->get_bounce());
|
||||
real_t jn = -(bounce + vn)*c.mass_normal;
|
||||
real_t jnOld = c.acc_normal_impulse;
|
||||
c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
|
||||
@@ -403,7 +435,6 @@ void BodyPair2DSW::solve(float p_step) {
|
||||
|
||||
Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld );
|
||||
|
||||
|
||||
A->apply_impulse(c.rA,-j);
|
||||
B->apply_impulse(c.rB, j);
|
||||
|
||||
|
||||
@@ -65,6 +65,7 @@ class BodyPair2DSW : public Constraint2DSW {
|
||||
real_t depth;
|
||||
bool active;
|
||||
Vector2 rA,rB;
|
||||
bool reused;
|
||||
};
|
||||
|
||||
Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
|
||||
@@ -76,7 +77,7 @@ class BodyPair2DSW : public Constraint2DSW {
|
||||
int cc;
|
||||
|
||||
|
||||
bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result=false);
|
||||
bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result=false);
|
||||
void _validate_contacts();
|
||||
static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self);
|
||||
_FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B);
|
||||
|
||||
@@ -378,7 +378,8 @@ int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
|
||||
return E->get().subindex;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
|
||||
template<bool use_aabb,bool use_segment>
|
||||
void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
|
||||
|
||||
|
||||
PosKey pk;
|
||||
@@ -411,10 +412,18 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re
|
||||
continue;
|
||||
|
||||
E->key()->pass=pass;
|
||||
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb))
|
||||
continue;
|
||||
|
||||
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
|
||||
continue;
|
||||
|
||||
p_results[index]=E->key()->owner;
|
||||
p_result_indices[index]=E->key()->subindex;
|
||||
index++;
|
||||
|
||||
|
||||
}
|
||||
|
||||
for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
|
||||
@@ -425,6 +434,12 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re
|
||||
if (E->key()->pass==pass)
|
||||
continue;
|
||||
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb))
|
||||
continue;
|
||||
|
||||
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
|
||||
continue;
|
||||
|
||||
E->key()->pass=pass;
|
||||
p_results[index]=E->key()->owner;
|
||||
p_result_indices[index]=E->key()->subindex;
|
||||
@@ -468,7 +483,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
|
||||
max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y;
|
||||
|
||||
int cullcount=0;
|
||||
_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
|
||||
_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
|
||||
|
||||
bool reached_x=false;
|
||||
bool reached_y=false;
|
||||
@@ -502,7 +517,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
|
||||
reached_y=true;
|
||||
}
|
||||
|
||||
_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
|
||||
_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
|
||||
|
||||
if (reached_x && reached_y)
|
||||
break;
|
||||
@@ -515,8 +530,22 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
|
||||
|
||||
int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
|
||||
|
||||
pass++;
|
||||
|
||||
return 0;
|
||||
Point2i from = (p_aabb.pos/cell_size).floor();
|
||||
Point2i to = ((p_aabb.pos+p_aabb.size)/cell_size).floor();
|
||||
int cullcount=0;
|
||||
|
||||
for(int i=from.x;i<=to.x;i++) {
|
||||
|
||||
for(int j=from.y;j<=to.y;j++) {
|
||||
|
||||
_cull<true,false>(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return cullcount;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
|
||||
|
||||
@@ -94,7 +94,8 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
|
||||
|
||||
void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
|
||||
void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
|
||||
_FORCE_INLINE_ void _cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
|
||||
template<bool use_aabb,bool use_segment>
|
||||
_FORCE_INLINE_ void _cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
|
||||
|
||||
|
||||
struct PosKey {
|
||||
|
||||
@@ -130,6 +130,7 @@ void CollisionObject2DSW::_update_shapes() {
|
||||
if (!space)
|
||||
return;
|
||||
|
||||
|
||||
for(int i=0;i<shapes.size();i++) {
|
||||
|
||||
Shape &s=shapes[i];
|
||||
|
||||
@@ -55,8 +55,10 @@ private:
|
||||
BroadPhase2DSW::ID bpid;
|
||||
Rect2 aabb_cache; //for rayqueries
|
||||
Shape2DSW *shape;
|
||||
Vector2 kinematic_advance;
|
||||
float kinematic_retreat;
|
||||
bool trigger;
|
||||
Shape() { trigger=false; }
|
||||
Shape() { trigger=false; kinematic_retreat=0; }
|
||||
};
|
||||
|
||||
Vector<Shape> shapes;
|
||||
@@ -73,7 +75,7 @@ protected:
|
||||
void _update_shapes_with_motion(const Vector2& p_motion);
|
||||
void _unregister_shapes();
|
||||
|
||||
_FORCE_INLINE_ void _set_transform(const Matrix32& p_transform) { transform=p_transform; _update_shapes(); }
|
||||
_FORCE_INLINE_ void _set_transform(const Matrix32& p_transform, bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) {_update_shapes();} }
|
||||
_FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; }
|
||||
void _set_static(bool p_static);
|
||||
|
||||
@@ -101,6 +103,12 @@ public:
|
||||
_FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
|
||||
_FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
|
||||
|
||||
_FORCE_INLINE_ void set_shape_kinematic_advance(int p_index,const Vector2& p_advance) { shapes[p_index].kinematic_advance=p_advance; }
|
||||
_FORCE_INLINE_ Vector2 get_shape_kinematic_advance(int p_index) const { return shapes[p_index].kinematic_advance; }
|
||||
|
||||
_FORCE_INLINE_ void set_shape_kinematic_retreat(int p_index,float p_retreat) { shapes[p_index].kinematic_retreat=p_retreat; }
|
||||
_FORCE_INLINE_ float get_shape_kinematic_retreat(int p_index) const { return shapes[p_index].kinematic_retreat; }
|
||||
|
||||
_FORCE_INLINE_ Matrix32 get_transform() const { return transform; }
|
||||
_FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; }
|
||||
_FORCE_INLINE_ Space2DSW* get_space() const { return space; }
|
||||
@@ -109,6 +117,7 @@ public:
|
||||
_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
|
||||
|
||||
|
||||
|
||||
void remove_shape(Shape2DSW *p_shape);
|
||||
void remove_shape(int p_index);
|
||||
|
||||
|
||||
@@ -321,19 +321,19 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po
|
||||
|
||||
|
||||
|
||||
template<class ShapeA, class ShapeB>
|
||||
template<class ShapeA, class ShapeB,bool castA=false,bool castB=false>
|
||||
class SeparatorAxisTest2D {
|
||||
|
||||
const ShapeA *shape_A;
|
||||
const ShapeB *shape_B;
|
||||
const Matrix32 *transform_A;
|
||||
const Matrix32 *transform_B;
|
||||
const Matrix32 *transform_inv_A;
|
||||
const Matrix32 *transform_inv_B;
|
||||
real_t best_depth;
|
||||
Vector2 best_axis;
|
||||
int best_axis_count;
|
||||
int best_axis_index;
|
||||
Vector2 motion_A;
|
||||
Vector2 motion_B;
|
||||
_CollectorCallback2D *callback;
|
||||
|
||||
public:
|
||||
@@ -351,6 +351,29 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ bool test_cast() {
|
||||
|
||||
if (castA) {
|
||||
|
||||
Vector2 na = motion_A.normalized();
|
||||
if (!test_axis(na))
|
||||
return false;
|
||||
if (!test_axis(na.tangent()))
|
||||
return false;
|
||||
}
|
||||
|
||||
if (castB) {
|
||||
|
||||
Vector2 nb = motion_B.normalized();
|
||||
if (!test_axis(nb))
|
||||
return false;
|
||||
if (!test_axis(nb.tangent()))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
|
||||
|
||||
Vector2 axis=p_axis;
|
||||
@@ -364,8 +387,15 @@ public:
|
||||
|
||||
real_t min_A,max_A,min_B,max_B;
|
||||
|
||||
shape_A->project_range(axis,*transform_A,min_A,max_A);
|
||||
shape_B->project_range(axis,*transform_B,min_B,max_B);
|
||||
if (castA)
|
||||
shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A);
|
||||
else
|
||||
shape_A->project_range(axis,*transform_A,min_A,max_A);
|
||||
|
||||
if (castB)
|
||||
shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B);
|
||||
else
|
||||
shape_B->project_range(axis,*transform_B,min_B,max_B);
|
||||
|
||||
min_B -= ( max_A - min_A ) * 0.5;
|
||||
max_B += ( max_A - min_A ) * 0.5;
|
||||
@@ -427,21 +457,28 @@ public:
|
||||
return; //only collide, no callback
|
||||
static const int max_supports=2;
|
||||
|
||||
|
||||
Vector2 supports_A[max_supports];
|
||||
int support_count_A;
|
||||
shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
|
||||
for(int i=0;i<support_count_A;i++) {
|
||||
supports_A[i] = transform_A->xform(supports_A[i]);
|
||||
if (castA) {
|
||||
shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A);
|
||||
} else {
|
||||
shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
|
||||
for(int i=0;i<support_count_A;i++) {
|
||||
supports_A[i] = transform_A->xform(supports_A[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Vector2 supports_B[max_supports];
|
||||
int support_count_B;
|
||||
shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
|
||||
for(int i=0;i<support_count_B;i++) {
|
||||
supports_B[i] = transform_B->xform(supports_B[i]);
|
||||
if (castB) {
|
||||
shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B);
|
||||
} else {
|
||||
shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
|
||||
for(int i=0;i<support_count_B;i++) {
|
||||
supports_B[i] = transform_B->xform(supports_B[i]);
|
||||
}
|
||||
}
|
||||
/*
|
||||
|
||||
@@ -480,14 +517,15 @@ public:
|
||||
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2()) {
|
||||
best_depth=1e15;
|
||||
shape_A=p_shape_A;
|
||||
shape_B=p_shape_B;
|
||||
transform_A=&p_transform_a;
|
||||
transform_B=&p_transform_b;
|
||||
transform_inv_A=&p_transform_inv_a;
|
||||
transform_inv_B=&p_transform_inv_b;
|
||||
motion_A=p_motion_A;
|
||||
motion_B=p_motion_B;
|
||||
|
||||
callback=p_collector;
|
||||
#ifdef DEBUG_ENABLED
|
||||
best_axis_count=0;
|
||||
@@ -503,68 +541,92 @@ public:
|
||||
/****** SAT TESTS *******/
|
||||
|
||||
|
||||
typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector);
|
||||
#define TEST_POINT(m_a,m_b) \
|
||||
( (!separator.test_axis(((m_a)-(m_b)).normalized())) ||\
|
||||
(castA && !separator.test_axis(((m_a)+p_motion_a-(m_b)).normalized())) ||\
|
||||
(castB && !separator.test_axis(((m_a)-((m_b)+p_motion_b)).normalized())) ||\
|
||||
(castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) )
|
||||
|
||||
static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
|
||||
typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&);
|
||||
|
||||
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
|
||||
const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
//this collision is kind of pointless
|
||||
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized()))
|
||||
|
||||
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
|
||||
return;
|
||||
if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b)))
|
||||
return;
|
||||
|
||||
separator.generate_contacts();
|
||||
|
||||
}
|
||||
|
||||
static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
|
||||
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
|
||||
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//segment normal
|
||||
if (!separator.test_axis(
|
||||
(p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent()
|
||||
))
|
||||
return;
|
||||
|
||||
// if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
|
||||
// return;
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized()))
|
||||
//endpoint a vs circle
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin()))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized()))
|
||||
//endpoint b vs circle
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin()))
|
||||
return;
|
||||
|
||||
|
||||
separator.generate_contacts();
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
|
||||
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
|
||||
@@ -577,50 +639,58 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_
|
||||
|
||||
}
|
||||
|
||||
static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
|
||||
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
|
||||
return;
|
||||
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
|
||||
return;
|
||||
|
||||
separator.generate_contacts();
|
||||
}
|
||||
|
||||
static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
|
||||
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
|
||||
return;
|
||||
|
||||
for(int i=0;i<convex_B->get_point_count();i++) {
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -631,35 +701,44 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3
|
||||
|
||||
/////////
|
||||
|
||||
static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
|
||||
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized()))
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin()))
|
||||
return;
|
||||
|
||||
|
||||
separator.generate_contacts();
|
||||
|
||||
}
|
||||
|
||||
static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
|
||||
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
const Vector2 &sphere=p_transform_a.elements[2];
|
||||
const Vector2 *axis=&p_transform_b.elements[0];
|
||||
const Vector2& half_extents = rectangle_B->get_half_extents();
|
||||
@@ -670,65 +749,120 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
|
||||
if (!separator.test_axis(axis[1].normalized()))
|
||||
return;
|
||||
|
||||
Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin());
|
||||
{
|
||||
Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin());
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
|
||||
if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
if (castA) {
|
||||
|
||||
Vector2 sphereofs = sphere + p_motion_a;
|
||||
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
|
||||
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
if (castB) {
|
||||
|
||||
Vector2 sphereofs = sphere - p_motion_b;
|
||||
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
|
||||
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
if (castA && castB) {
|
||||
|
||||
Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
|
||||
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
|
||||
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
separator.generate_contacts();
|
||||
}
|
||||
|
||||
static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
|
||||
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//capsule axis
|
||||
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
|
||||
return;
|
||||
|
||||
//capsule endpoints
|
||||
if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
|
||||
return;
|
||||
if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
|
||||
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
|
||||
return;
|
||||
|
||||
|
||||
separator.generate_contacts();
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
|
||||
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
|
||||
//poly faces and poly points vs circle
|
||||
for(int i=0;i<convex_B->get_point_count();i++) {
|
||||
|
||||
if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() ))
|
||||
if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) ))
|
||||
return;
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -738,17 +872,21 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32
|
||||
|
||||
/////////
|
||||
|
||||
static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
|
||||
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//box faces A
|
||||
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
|
||||
return;
|
||||
@@ -766,17 +904,21 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32&
|
||||
separator.generate_contacts();
|
||||
}
|
||||
|
||||
static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
|
||||
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//box faces
|
||||
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
|
||||
return;
|
||||
@@ -791,21 +933,77 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
|
||||
|
||||
//box endpoints to capsule circles
|
||||
|
||||
Matrix32 boxinv = p_transform_a.affine_inverse();
|
||||
|
||||
for(int i=0;i<2;i++) {
|
||||
|
||||
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
|
||||
{
|
||||
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
|
||||
|
||||
const Vector2& half_extents = rectangle_A->get_half_extents();
|
||||
Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint);
|
||||
const Vector2& half_extents = rectangle_A->get_half_extents();
|
||||
Vector2 local_v = boxinv.xform(capsule_endpoint);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if (!separator.test_axis(p_transform_a.xform(he).normalized()))
|
||||
return;
|
||||
if (castA) {
|
||||
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
|
||||
capsule_endpoint-=p_motion_a;
|
||||
|
||||
|
||||
const Vector2& half_extents = rectangle_A->get_half_extents();
|
||||
Vector2 local_v = boxinv.xform(capsule_endpoint);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
if (castB) {
|
||||
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
|
||||
capsule_endpoint+=p_motion_b;
|
||||
|
||||
|
||||
const Vector2& half_extents = rectangle_A->get_half_extents();
|
||||
Vector2 local_v = boxinv.xform(capsule_endpoint);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
if (castA && castB) {
|
||||
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
|
||||
capsule_endpoint-=p_motion_a;
|
||||
capsule_endpoint+=p_motion_b;
|
||||
|
||||
|
||||
const Vector2& half_extents = rectangle_A->get_half_extents();
|
||||
Vector2 local_v = boxinv.xform(capsule_endpoint);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -813,16 +1011,20 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
|
||||
separator.generate_contacts();
|
||||
}
|
||||
|
||||
static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
|
||||
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//box faces
|
||||
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
|
||||
return;
|
||||
@@ -833,7 +1035,7 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
|
||||
//convex faces
|
||||
for(int i=0;i<convex_B->get_point_count();i++) {
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -844,17 +1046,21 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
|
||||
|
||||
/////////
|
||||
|
||||
static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
|
||||
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//capsule axis
|
||||
|
||||
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
|
||||
@@ -873,7 +1079,7 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
|
||||
|
||||
Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
|
||||
|
||||
if (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() ))
|
||||
if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
|
||||
return;
|
||||
|
||||
}
|
||||
@@ -883,17 +1089,21 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
|
||||
|
||||
}
|
||||
|
||||
static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
|
||||
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
|
||||
|
||||
|
||||
SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
//capsule axis
|
||||
|
||||
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
|
||||
@@ -909,12 +1119,12 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
|
||||
|
||||
Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
|
||||
|
||||
if (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() ))
|
||||
if (TEST_POINT(capsule_endpoint_A,cpoint ))
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -925,26 +1135,31 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
|
||||
/////////
|
||||
|
||||
|
||||
static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
|
||||
template<bool castA, bool castB>
|
||||
static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
|
||||
|
||||
|
||||
const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
|
||||
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
|
||||
|
||||
SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
|
||||
SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
|
||||
|
||||
if (!separator.test_previous_axis())
|
||||
return;
|
||||
|
||||
if (!separator.test_cast())
|
||||
return;
|
||||
|
||||
|
||||
for(int i=0;i<convex_A->get_point_count();i++) {
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
for(int i=0;i<convex_B->get_point_count();i++) {
|
||||
|
||||
if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
|
||||
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -955,7 +1170,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const
|
||||
|
||||
////////
|
||||
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
|
||||
|
||||
Physics2DServer::ShapeType type_A=p_shape_A->get_type();
|
||||
|
||||
@@ -971,31 +1186,118 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
|
||||
|
||||
|
||||
static const CollisionFunc collision_table[5][5]={
|
||||
{_collision_segment_segment,
|
||||
_collision_segment_circle,
|
||||
_collision_segment_rectangle,
|
||||
_collision_segment_capsule,
|
||||
_collision_segment_convex_polygon},
|
||||
{_collision_segment_segment<false,false>,
|
||||
_collision_segment_circle<false,false>,
|
||||
_collision_segment_rectangle<false,false>,
|
||||
_collision_segment_capsule<false,false>,
|
||||
_collision_segment_convex_polygon<false,false>},
|
||||
{0,
|
||||
_collision_circle_circle,
|
||||
_collision_circle_rectangle,
|
||||
_collision_circle_capsule,
|
||||
_collision_circle_convex_polygon},
|
||||
_collision_circle_circle<false,false>,
|
||||
_collision_circle_rectangle<false,false>,
|
||||
_collision_circle_capsule<false,false>,
|
||||
_collision_circle_convex_polygon<false,false>},
|
||||
{0,
|
||||
0,
|
||||
_collision_rectangle_rectangle,
|
||||
_collision_rectangle_capsule,
|
||||
_collision_rectangle_convex_polygon},
|
||||
_collision_rectangle_rectangle<false,false>,
|
||||
_collision_rectangle_capsule<false,false>,
|
||||
_collision_rectangle_convex_polygon<false,false>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
_collision_capsule_capsule,
|
||||
_collision_capsule_convex_polygon},
|
||||
_collision_capsule_capsule<false,false>,
|
||||
_collision_capsule_convex_polygon<false,false>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
_collision_convex_polygon_convex_polygon}
|
||||
_collision_convex_polygon_convex_polygon<false,false>}
|
||||
|
||||
};
|
||||
|
||||
static const CollisionFunc collision_table_castA[5][5]={
|
||||
{_collision_segment_segment<true,false>,
|
||||
_collision_segment_circle<true,false>,
|
||||
_collision_segment_rectangle<true,false>,
|
||||
_collision_segment_capsule<true,false>,
|
||||
_collision_segment_convex_polygon<true,false>},
|
||||
{0,
|
||||
_collision_circle_circle<true,false>,
|
||||
_collision_circle_rectangle<true,false>,
|
||||
_collision_circle_capsule<true,false>,
|
||||
_collision_circle_convex_polygon<true,false>},
|
||||
{0,
|
||||
0,
|
||||
_collision_rectangle_rectangle<true,false>,
|
||||
_collision_rectangle_capsule<true,false>,
|
||||
_collision_rectangle_convex_polygon<true,false>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
_collision_capsule_capsule<true,false>,
|
||||
_collision_capsule_convex_polygon<true,false>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
_collision_convex_polygon_convex_polygon<true,false>}
|
||||
|
||||
};
|
||||
|
||||
static const CollisionFunc collision_table_castB[5][5]={
|
||||
{_collision_segment_segment<false,true>,
|
||||
_collision_segment_circle<false,true>,
|
||||
_collision_segment_rectangle<false,true>,
|
||||
_collision_segment_capsule<false,true>,
|
||||
_collision_segment_convex_polygon<false,true>},
|
||||
{0,
|
||||
_collision_circle_circle<false,true>,
|
||||
_collision_circle_rectangle<false,true>,
|
||||
_collision_circle_capsule<false,true>,
|
||||
_collision_circle_convex_polygon<false,true>},
|
||||
{0,
|
||||
0,
|
||||
_collision_rectangle_rectangle<false,true>,
|
||||
_collision_rectangle_capsule<false,true>,
|
||||
_collision_rectangle_convex_polygon<false,true>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
_collision_capsule_capsule<false,true>,
|
||||
_collision_capsule_convex_polygon<false,true>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
_collision_convex_polygon_convex_polygon<false,true>}
|
||||
|
||||
};
|
||||
|
||||
static const CollisionFunc collision_table_castA_castB[5][5]={
|
||||
{_collision_segment_segment<true,true>,
|
||||
_collision_segment_circle<true,true>,
|
||||
_collision_segment_rectangle<true,true>,
|
||||
_collision_segment_capsule<true,true>,
|
||||
_collision_segment_convex_polygon<true,true>},
|
||||
{0,
|
||||
_collision_circle_circle<true,true>,
|
||||
_collision_circle_rectangle<true,true>,
|
||||
_collision_circle_capsule<true,true>,
|
||||
_collision_circle_convex_polygon<true,true>},
|
||||
{0,
|
||||
0,
|
||||
_collision_rectangle_rectangle<true,true>,
|
||||
_collision_rectangle_capsule<true,true>,
|
||||
_collision_rectangle_convex_polygon<true,true>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
_collision_capsule_capsule<true,true>,
|
||||
_collision_capsule_convex_polygon<true,true>},
|
||||
{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
_collision_convex_polygon_convex_polygon<true,true>}
|
||||
|
||||
};
|
||||
|
||||
@@ -1010,23 +1312,34 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
|
||||
const Shape2DSW *B=p_shape_B;
|
||||
const Matrix32 *transform_A=&p_transform_A;
|
||||
const Matrix32 *transform_B=&p_transform_B;
|
||||
const Matrix32 *transform_inv_A=&p_transform_inv_A;
|
||||
const Matrix32 *transform_inv_B=&p_transform_inv_B;
|
||||
const Vector2 *motion_A=&p_motion_A;
|
||||
const Vector2 *motion_B=&p_motion_B;
|
||||
|
||||
if (type_A > type_B) {
|
||||
SWAP(A,B);
|
||||
SWAP(transform_A,transform_B);
|
||||
SWAP(transform_inv_A,transform_inv_B);
|
||||
SWAP(type_A,type_B);
|
||||
SWAP(motion_A,motion_B);
|
||||
callback.swap = !callback.swap;
|
||||
}
|
||||
|
||||
|
||||
CollisionFunc collision_func = collision_table[type_A-2][type_B-2];
|
||||
CollisionFunc collision_func;
|
||||
if (*motion_A==Vector2() && *motion_B==Vector2()) {
|
||||
collision_func = collision_table[type_A-2][type_B-2];
|
||||
} else if (*motion_A!=Vector2() && *motion_B==Vector2()) {
|
||||
collision_func = collision_table_castA[type_A-2][type_B-2];
|
||||
} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
|
||||
collision_func = collision_table_castB[type_A-2][type_B-2];
|
||||
} else {
|
||||
collision_func = collision_table_castA_castB[type_A-2][type_B-2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
ERR_FAIL_COND_V(!collision_func,false);
|
||||
|
||||
|
||||
collision_func(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback);
|
||||
collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B);
|
||||
|
||||
return callback.collided;
|
||||
|
||||
|
||||
@@ -32,6 +32,6 @@
|
||||
#include "collision_solver_2d_sw.h"
|
||||
|
||||
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
|
||||
|
||||
#endif // COLLISION_SOLVER_2D_SAT_H
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
//#define collision_solver gjk_epa_calculate_penetration
|
||||
|
||||
|
||||
bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
|
||||
bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
|
||||
|
||||
|
||||
const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
|
||||
@@ -49,7 +49,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat
|
||||
Vector2 supports[2];
|
||||
int support_count;
|
||||
|
||||
p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count);
|
||||
p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count);
|
||||
|
||||
bool found=false;
|
||||
|
||||
@@ -77,7 +77,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat
|
||||
return found;
|
||||
}
|
||||
|
||||
bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
|
||||
bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
|
||||
|
||||
|
||||
|
||||
@@ -89,8 +89,9 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix3
|
||||
Vector2 to = from+p_transform_A[1]*ray->get_length();
|
||||
Vector2 support_A=to;
|
||||
|
||||
from = p_transform_inv_B.xform(from);
|
||||
to = p_transform_inv_B.xform(to);
|
||||
Matrix32 invb = p_transform_B.affine_inverse();
|
||||
from = invb.xform(from);
|
||||
to = invb.xform(to);
|
||||
|
||||
Vector2 p,n;
|
||||
if (!p_shape_B->intersect_segment(from,to,p,n)) {
|
||||
@@ -145,10 +146,10 @@ bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p
|
||||
struct _ConcaveCollisionInfo2D {
|
||||
|
||||
const Matrix32 *transform_A;
|
||||
const Matrix32 *transform_inv_A;
|
||||
const Shape2DSW *shape_A;
|
||||
const Matrix32 *transform_B;
|
||||
const Matrix32 *transform_inv_B;
|
||||
Vector2 motion_A;
|
||||
Vector2 motion_B;
|
||||
CollisionSolver2DSW::CallbackResult result_callback;
|
||||
void *userdata;
|
||||
bool swap_result;
|
||||
@@ -168,7 +169,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
|
||||
if (!cinfo.result_callback && cinfo.collided)
|
||||
return; //already collided and no contacts requested, don't test anymore
|
||||
|
||||
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, *cinfo.transform_inv_A, p_convex,*cinfo.transform_B,*cinfo.transform_inv_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
|
||||
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
|
||||
if (!collided)
|
||||
return;
|
||||
|
||||
@@ -178,17 +179,16 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
|
||||
|
||||
}
|
||||
|
||||
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
|
||||
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
|
||||
|
||||
|
||||
const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
|
||||
|
||||
_ConcaveCollisionInfo2D cinfo;
|
||||
cinfo.transform_A=&p_transform_A;
|
||||
cinfo.transform_inv_A=&p_transform_inv_A;
|
||||
cinfo.shape_A=p_shape_A;
|
||||
cinfo.transform_B=&p_transform_B;
|
||||
cinfo.transform_inv_B=&p_transform_inv_B;
|
||||
cinfo.motion_A=p_motion_A;
|
||||
cinfo.result_callback=p_result_callback;
|
||||
cinfo.userdata=p_userdata;
|
||||
cinfo.swap_result=p_swap_result;
|
||||
@@ -227,7 +227,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
|
||||
}
|
||||
|
||||
|
||||
bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
|
||||
bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -253,9 +254,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
|
||||
}
|
||||
|
||||
if (swap) {
|
||||
return solve_static_line(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true);
|
||||
return solve_static_line(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
|
||||
} else {
|
||||
return solve_static_line(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false);
|
||||
return solve_static_line(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
|
||||
}
|
||||
|
||||
/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
|
||||
@@ -278,9 +279,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
|
||||
|
||||
|
||||
if (swap) {
|
||||
return solve_raycast(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
|
||||
return solve_raycast(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,sep_axis);
|
||||
} else {
|
||||
return solve_raycast(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
return solve_raycast(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
}
|
||||
|
||||
|
||||
@@ -291,16 +292,16 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
|
||||
return false;
|
||||
|
||||
if (!swap)
|
||||
return solve_concave(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
else
|
||||
return solve_concave(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
|
||||
return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis);
|
||||
|
||||
|
||||
|
||||
} else {
|
||||
|
||||
|
||||
return collision_solver(p_shape_A, p_transform_A, p_transform_inv_A, p_shape_B, p_transform_B, p_transform_inv_B, p_result_callback,p_userdata,false,sep_axis);
|
||||
return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -35,16 +35,16 @@ class CollisionSolver2DSW {
|
||||
public:
|
||||
typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
|
||||
private:
|
||||
static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
|
||||
static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
|
||||
static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
|
||||
static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
|
||||
static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
|
||||
static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
|
||||
static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
static bool solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_inverse_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
|
||||
static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -29,6 +29,7 @@
|
||||
#include "physics_2d_server_sw.h"
|
||||
#include "broad_phase_2d_basic.h"
|
||||
#include "broad_phase_2d_hash_grid.h"
|
||||
#include "collision_solver_2d_sw.h"
|
||||
|
||||
RID Physics2DServerSW::shape_create(ShapeType p_shape) {
|
||||
|
||||
@@ -81,6 +82,9 @@ RID Physics2DServerSW::shape_create(ShapeType p_shape) {
|
||||
return id;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
|
||||
|
||||
Shape2DSW *shape = shape_owner.get(p_shape);
|
||||
@@ -126,6 +130,63 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
|
||||
}
|
||||
|
||||
|
||||
void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
|
||||
|
||||
CollCbkData *cbk=(CollCbkData *)p_userdata;
|
||||
|
||||
if (cbk->amount == cbk->max) {
|
||||
//find least deep
|
||||
float min_depth=1e20;
|
||||
int min_depth_idx=0;
|
||||
for(int i=0;i<cbk->amount;i++) {
|
||||
|
||||
float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]);
|
||||
if (d<min_depth) {
|
||||
min_depth=d;
|
||||
min_depth_idx=i;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float d = p_point_A.distance_squared_to(p_point_B);
|
||||
if (d<min_depth)
|
||||
return;
|
||||
cbk->ptr[min_depth_idx*2+0]=p_point_A;
|
||||
cbk->ptr[min_depth_idx*2+1]=p_point_B;
|
||||
|
||||
|
||||
} else {
|
||||
|
||||
cbk->ptr[cbk->amount*2+0]=p_point_A;
|
||||
cbk->ptr[cbk->amount*2+1]=p_point_B;
|
||||
}
|
||||
}
|
||||
|
||||
bool Physics2DServerSW::shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) {
|
||||
|
||||
|
||||
Shape2DSW *shape_A = shape_owner.get(p_shape_A);
|
||||
ERR_FAIL_COND_V(!shape_A,false);
|
||||
Shape2DSW *shape_B = shape_owner.get(p_shape_B);
|
||||
ERR_FAIL_COND_V(!shape_B,false);
|
||||
|
||||
if (p_result_max==0) {
|
||||
|
||||
return CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,NULL,NULL);
|
||||
}
|
||||
|
||||
CollCbkData cbk;
|
||||
cbk.max=p_result_max;
|
||||
cbk.amount=0;
|
||||
cbk.ptr=r_results;
|
||||
|
||||
bool res= CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,_shape_col_cbk,&cbk);
|
||||
r_result_count=cbk.amount;
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
|
||||
RID Physics2DServerSW::space_create() {
|
||||
|
||||
Space2DSW *space = memnew( Space2DSW );
|
||||
@@ -442,7 +503,7 @@ void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
|
||||
body->set_mode(p_mode);
|
||||
};
|
||||
|
||||
Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body, BodyMode p_mode) const {
|
||||
Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body) const {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
|
||||
@@ -550,23 +611,25 @@ bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx
|
||||
}
|
||||
|
||||
|
||||
void Physics2DServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) {
|
||||
void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
|
||||
body->set_continuous_collision_detection(p_enable);
|
||||
body->set_continuous_collision_detection_mode(p_mode);
|
||||
|
||||
}
|
||||
|
||||
bool Physics2DServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
|
||||
Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const{
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,false);
|
||||
const Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,CCD_MODE_DISABLED);
|
||||
|
||||
return body->get_continuous_collision_detection_mode();
|
||||
|
||||
return body->is_continuous_collision_detection_enabled();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
@@ -617,13 +680,6 @@ float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const
|
||||
};
|
||||
|
||||
|
||||
void Physics2DServerSW::body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
body->simulate_motion(p_new_transform,last_step);
|
||||
|
||||
};
|
||||
|
||||
void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
|
||||
|
||||
@@ -775,6 +831,16 @@ void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p
|
||||
|
||||
}
|
||||
|
||||
bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,false);
|
||||
ERR_FAIL_INDEX_V(p_body_shape,body->get_shape_count(),false);
|
||||
|
||||
return shape_collide(body->get_shape(p_body_shape)->get_self(),body->get_transform() * body->get_shape_transform(p_body_shape),Vector2(),p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
|
||||
@@ -58,6 +58,16 @@ friend class Physics2DDirectSpaceStateSW;
|
||||
mutable RID_Owner<Body2DSW> body_owner;
|
||||
mutable RID_Owner<Joint2DSW> joint_owner;
|
||||
|
||||
struct CollCbkData {
|
||||
|
||||
int max;
|
||||
int amount;
|
||||
Vector2 *ptr;
|
||||
};
|
||||
|
||||
static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
|
||||
|
||||
|
||||
// void _clear_query(Query2DSW *p_query);
|
||||
public:
|
||||
|
||||
@@ -69,6 +79,8 @@ public:
|
||||
virtual Variant shape_get_data(RID p_shape) const;
|
||||
virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
|
||||
|
||||
virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count);
|
||||
|
||||
/* SPACE API */
|
||||
|
||||
virtual RID space_create();
|
||||
@@ -124,7 +136,7 @@ public:
|
||||
virtual RID body_get_space(RID p_body) const;
|
||||
|
||||
virtual void body_set_mode(RID p_body, BodyMode p_mode);
|
||||
virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const;
|
||||
virtual BodyMode body_get_mode(RID p_body) const;
|
||||
|
||||
virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32());
|
||||
virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
|
||||
@@ -143,8 +155,8 @@ public:
|
||||
virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
|
||||
virtual uint32_t body_get_object_instance_ID(RID p_body) const;
|
||||
|
||||
virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
|
||||
virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
|
||||
virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
|
||||
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
|
||||
|
||||
virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
|
||||
virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
|
||||
@@ -152,8 +164,6 @@ public:
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
|
||||
virtual float body_get_param(RID p_body, BodyParameter p_param) const;
|
||||
|
||||
//advanced simulation
|
||||
virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform);
|
||||
|
||||
virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
|
||||
virtual Variant body_get_state(RID p_body, BodyState p_state) const;
|
||||
@@ -181,6 +191,7 @@ public:
|
||||
virtual int body_get_max_contacts_reported(RID p_body) const;
|
||||
|
||||
virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
|
||||
virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count);
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -26,417 +26,516 @@
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
#ifndef SHAPE_2D_2DSW_H
|
||||
#define SHAPE_2D_2DSW_H
|
||||
|
||||
#include "servers/physics_2d_server.h"
|
||||
/*
|
||||
|
||||
SHAPE_LINE, ///< plane:"plane"
|
||||
SHAPE_SEGMENT, ///< float:"length"
|
||||
SHAPE_CIRCLE, ///< float:"radius"
|
||||
SHAPE_RECTANGLE, ///< vec3:"extents"
|
||||
SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
|
||||
SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
|
||||
SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
|
||||
|
||||
*/
|
||||
|
||||
class Shape2DSW;
|
||||
|
||||
class ShapeOwner2DSW {
|
||||
public:
|
||||
|
||||
virtual void _shape_changed()=0;
|
||||
virtual void remove_shape(Shape2DSW *p_shape)=0;
|
||||
|
||||
virtual ~ShapeOwner2DSW() {}
|
||||
};
|
||||
|
||||
class Shape2DSW {
|
||||
|
||||
RID self;
|
||||
Rect2 aabb;
|
||||
bool configured;
|
||||
real_t custom_bias;
|
||||
|
||||
Map<ShapeOwner2DSW*,int> owners;
|
||||
protected:
|
||||
|
||||
void configure(const Rect2& p_aabb);
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const {return self; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const=0;
|
||||
|
||||
_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
|
||||
_FORCE_INLINE_ bool is_configured() const { return configured; }
|
||||
|
||||
virtual bool is_concave() const { return false; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
|
||||
virtual Vector2 get_support(const Vector2& p_normal) const;
|
||||
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 void set_data(const Variant& p_data)=0;
|
||||
virtual Variant get_data() const=0;
|
||||
|
||||
_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
|
||||
_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
|
||||
|
||||
void add_owner(ShapeOwner2DSW *p_owner);
|
||||
void remove_owner(ShapeOwner2DSW *p_owner);
|
||||
bool is_owner(ShapeOwner2DSW *p_owner) const;
|
||||
const Map<ShapeOwner2DSW*,int>& get_owners() const;
|
||||
|
||||
Shape2DSW();
|
||||
virtual ~Shape2DSW();
|
||||
};
|
||||
|
||||
|
||||
class LineShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 normal;
|
||||
real_t d;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
|
||||
_FORCE_INLINE_ real_t get_d() const { return d; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_min=-1e10;
|
||||
r_max=1e10;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
class RayShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t length;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
_FORCE_INLINE_ real_t get_length() const { return length; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_max = p_normal.dot(p_transform.get_origin());
|
||||
r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ RayShape2DSW() {}
|
||||
_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
|
||||
};
|
||||
|
||||
|
||||
class SegmentShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 a;
|
||||
Vector2 b;
|
||||
Vector2 n;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
_FORCE_INLINE_ const Vector2& get_a() const { return a; }
|
||||
_FORCE_INLINE_ const Vector2& get_b() const { return b; }
|
||||
_FORCE_INLINE_ const Vector2& get_normal() const { return n; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_max = p_normal.dot(p_transform.xform(a));
|
||||
r_min = p_normal.dot(p_transform.xform(b));
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ SegmentShape2DSW() {}
|
||||
_FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
|
||||
};
|
||||
|
||||
|
||||
class CircleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t radius;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
real_t d = p_normal.dot( p_transform.get_origin() );
|
||||
|
||||
// figure out scale at point
|
||||
Vector2 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;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class RectangleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 half_extents;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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);
|
||||
|
||||
float length = local_normal.abs().dot(half_extents);
|
||||
float distance = p_normal.dot( p_transform.get_origin() );
|
||||
|
||||
r_min = distance - length;
|
||||
r_max = distance + length;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class CapsuleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t radius;
|
||||
real_t height;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
|
||||
_FORCE_INLINE_ const real_t& get_height() const { return height; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 n=p_transform.basis_xform_inv(p_normal).normalized();
|
||||
float h = (n.y > 0) ? height : -height;
|
||||
|
||||
n *= radius;
|
||||
n.y += h * 0.5;
|
||||
|
||||
r_max = p_normal.dot(p_transform.xform(n));
|
||||
r_min = p_normal.dot(p_transform.xform(-n));
|
||||
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
|
||||
//ERR_FAIL_COND( r_max < r_min );
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
class ConvexPolygonShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
struct Point {
|
||||
|
||||
Vector2 pos;
|
||||
Vector2 normal; //normal to next segment
|
||||
};
|
||||
|
||||
Point *points;
|
||||
int point_count;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ int get_point_count() const { return point_count; }
|
||||
_FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
|
||||
_FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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
|
||||
|
||||
r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
|
||||
for(int i=1;i<point_count;i++) {
|
||||
|
||||
float d = p_normal.dot(p_transform.xform(points[i].pos));
|
||||
if (d>r_max)
|
||||
r_max=d;
|
||||
if (d<r_min)
|
||||
r_min=d;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
ConvexPolygonShape2DSW();
|
||||
~ConvexPolygonShape2DSW();
|
||||
};
|
||||
|
||||
|
||||
class ConcaveShape2DSW : public Shape2DSW {
|
||||
|
||||
public:
|
||||
|
||||
virtual bool is_concave() const { return true; }
|
||||
typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
|
||||
|
||||
virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
|
||||
|
||||
};
|
||||
|
||||
class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
|
||||
|
||||
struct Segment {
|
||||
|
||||
int points[2];
|
||||
};
|
||||
|
||||
Vector<Segment> segments;
|
||||
Vector<Point2> points;
|
||||
|
||||
struct BVH {
|
||||
|
||||
Rect2 aabb;
|
||||
int left,right;
|
||||
};
|
||||
|
||||
|
||||
Vector<BVH> bvh;
|
||||
int bvh_depth;
|
||||
|
||||
|
||||
struct BVH_CompareX {
|
||||
|
||||
_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
|
||||
|
||||
return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
|
||||
}
|
||||
};
|
||||
|
||||
struct BVH_CompareY {
|
||||
|
||||
_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
|
||||
|
||||
return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
|
||||
}
|
||||
};
|
||||
|
||||
int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
|
||||
|
||||
public:
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
|
||||
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 { return 0; }
|
||||
|
||||
virtual void set_data(const Variant& p_data);
|
||||
virtual Variant get_data() const;
|
||||
|
||||
virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // SHAPE_2D_2DSW_H
|
||||
#ifndef SHAPE_2D_2DSW_H
|
||||
#define SHAPE_2D_2DSW_H
|
||||
|
||||
#include "servers/physics_2d_server.h"
|
||||
#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998
|
||||
|
||||
/*
|
||||
|
||||
SHAPE_LINE, ///< plane:"plane"
|
||||
SHAPE_SEGMENT, ///< float:"length"
|
||||
SHAPE_CIRCLE, ///< float:"radius"
|
||||
SHAPE_RECTANGLE, ///< vec3:"extents"
|
||||
SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
|
||||
SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
|
||||
SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
|
||||
|
||||
*/
|
||||
|
||||
class Shape2DSW;
|
||||
|
||||
class ShapeOwner2DSW {
|
||||
public:
|
||||
|
||||
virtual void _shape_changed()=0;
|
||||
virtual void remove_shape(Shape2DSW *p_shape)=0;
|
||||
|
||||
virtual ~ShapeOwner2DSW() {}
|
||||
};
|
||||
|
||||
class Shape2DSW {
|
||||
|
||||
RID self;
|
||||
Rect2 aabb;
|
||||
bool configured;
|
||||
real_t custom_bias;
|
||||
|
||||
Map<ShapeOwner2DSW*,int> owners;
|
||||
protected:
|
||||
|
||||
void configure(const Rect2& p_aabb);
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const {return self; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const=0;
|
||||
|
||||
_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
|
||||
_FORCE_INLINE_ bool is_configured() const { return configured; }
|
||||
|
||||
virtual bool is_concave() const { return false; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
|
||||
virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
|
||||
virtual Vector2 get_support(const Vector2& p_normal) const;
|
||||
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 void set_data(const Variant& p_data)=0;
|
||||
virtual Variant get_data() const=0;
|
||||
|
||||
_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
|
||||
_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
|
||||
|
||||
void add_owner(ShapeOwner2DSW *p_owner);
|
||||
void remove_owner(ShapeOwner2DSW *p_owner);
|
||||
bool is_owner(ShapeOwner2DSW *p_owner) const;
|
||||
const Map<ShapeOwner2DSW*,int>& get_owners() const;
|
||||
|
||||
|
||||
_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2& p_cast,const Vector2& p_normal,const Matrix32& p_xform,Vector2 *r_supports,int & r_amount) const {
|
||||
|
||||
get_supports(p_xform.basis_xform_inv(p_normal).normalized(),r_supports,r_amount);
|
||||
for(int i=0;i<r_amount;i++)
|
||||
r_supports[i]=p_xform.xform(r_supports[i]);
|
||||
|
||||
if (r_amount==1) {
|
||||
|
||||
if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
|
||||
//make line because they are parallel
|
||||
r_amount=2;
|
||||
r_supports[1]=r_supports[0]+p_cast;
|
||||
} else if (p_cast.dot(p_normal)>0) {
|
||||
//normal points towards cast, add cast
|
||||
r_supports[0]+=p_cast;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
|
||||
//optimize line and make it larger because they are parallel
|
||||
if ((r_supports[1]-r_supports[0]).dot(p_cast)>0) {
|
||||
//larger towards 1
|
||||
r_supports[1]+=p_cast;
|
||||
} else {
|
||||
//larger towards 0
|
||||
r_supports[0]+=p_cast;
|
||||
}
|
||||
} else if (p_cast.dot(p_normal)>0) {
|
||||
//normal points towards cast, add cast
|
||||
r_supports[0]+=p_cast;
|
||||
r_supports[1]+=p_cast;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Shape2DSW();
|
||||
virtual ~Shape2DSW();
|
||||
};
|
||||
|
||||
//let the optimizer do the magic
|
||||
#define DEFAULT_PROJECT_RANGE_CAST \
|
||||
virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
|
||||
project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);\
|
||||
}\
|
||||
_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
|
||||
\
|
||||
real_t mina,maxa;\
|
||||
real_t minb,maxb;\
|
||||
Matrix32 ofsb=p_transform;\
|
||||
ofsb.elements[2]+=p_cast;\
|
||||
project_range(p_normal,p_transform,mina,maxa);\
|
||||
project_range(p_normal,ofsb,minb,maxb); \
|
||||
r_min=MIN(mina,minb);\
|
||||
r_max=MAX(maxa,maxb);\
|
||||
}
|
||||
|
||||
class LineShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 normal;
|
||||
real_t d;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
|
||||
_FORCE_INLINE_ real_t get_d() const { return d; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_min=-1e10;
|
||||
r_max=1e10;
|
||||
}
|
||||
|
||||
virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
|
||||
project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
|
||||
//real large
|
||||
r_min=-1e10;
|
||||
r_max=1e10;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
class RayShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t length;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
_FORCE_INLINE_ real_t get_length() const { return length; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_max = p_normal.dot(p_transform.get_origin());
|
||||
r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
}
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
|
||||
_FORCE_INLINE_ RayShape2DSW() {}
|
||||
_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
|
||||
};
|
||||
|
||||
|
||||
class SegmentShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 a;
|
||||
Vector2 b;
|
||||
Vector2 n;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
_FORCE_INLINE_ const Vector2& get_a() const { return a; }
|
||||
_FORCE_INLINE_ const Vector2& get_b() const { return b; }
|
||||
_FORCE_INLINE_ const Vector2& get_normal() const { return n; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_xformed_normal(const Matrix32& p_xform) const {
|
||||
|
||||
return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent();
|
||||
}
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
r_max = p_normal.dot(p_transform.xform(a));
|
||||
r_min = p_normal.dot(p_transform.xform(b));
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
}
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
_FORCE_INLINE_ SegmentShape2DSW() {}
|
||||
_FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
|
||||
};
|
||||
|
||||
|
||||
class CircleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t radius;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 {
|
||||
//real large
|
||||
real_t d = p_normal.dot( p_transform.get_origin() );
|
||||
|
||||
// figure out scale at point
|
||||
Vector2 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;
|
||||
}
|
||||
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class RectangleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
Vector2 half_extents;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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);
|
||||
|
||||
float length = local_normal.abs().dot(half_extents);
|
||||
float distance = p_normal.dot( p_transform.get_origin() );
|
||||
|
||||
r_min = distance - length;
|
||||
r_max = distance + length;
|
||||
}
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
};
|
||||
|
||||
class CapsuleShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
real_t radius;
|
||||
real_t height;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
|
||||
_FORCE_INLINE_ const real_t& get_height() const { return height; }
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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 n=p_transform.basis_xform_inv(p_normal).normalized();
|
||||
float h = (n.y > 0) ? height : -height;
|
||||
|
||||
n *= radius;
|
||||
n.y += h * 0.5;
|
||||
|
||||
r_max = p_normal.dot(p_transform.xform(n));
|
||||
r_min = p_normal.dot(p_transform.xform(-n));
|
||||
|
||||
if (r_max<r_min) {
|
||||
|
||||
SWAP(r_max,r_min);
|
||||
}
|
||||
|
||||
//ERR_FAIL_COND( r_max < r_min );
|
||||
}
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
class ConvexPolygonShape2DSW : public Shape2DSW {
|
||||
|
||||
|
||||
struct Point {
|
||||
|
||||
Vector2 pos;
|
||||
Vector2 normal; //normal to next segment
|
||||
};
|
||||
|
||||
Point *points;
|
||||
int point_count;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ int get_point_count() const { return point_count; }
|
||||
_FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
|
||||
_FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
|
||||
_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Matrix32& p_xform, int p_idx) const {
|
||||
|
||||
Vector2 a = points[p_idx].pos;
|
||||
p_idx++;
|
||||
Vector2 b = points[p_idx==point_count?0:p_idx].pos;
|
||||
return (p_xform.xform(b)-p_xform.xform(a)).normalized().tangent();
|
||||
}
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
|
||||
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 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
|
||||
|
||||
r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
|
||||
for(int i=1;i<point_count;i++) {
|
||||
|
||||
float d = p_normal.dot(p_transform.xform(points[i].pos));
|
||||
if (d>r_max)
|
||||
r_max=d;
|
||||
if (d<r_min)
|
||||
r_min=d;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
ConvexPolygonShape2DSW();
|
||||
~ConvexPolygonShape2DSW();
|
||||
};
|
||||
|
||||
|
||||
class ConcaveShape2DSW : public Shape2DSW {
|
||||
|
||||
public:
|
||||
|
||||
virtual bool is_concave() const { return true; }
|
||||
typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
|
||||
|
||||
virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
|
||||
|
||||
};
|
||||
|
||||
class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
|
||||
|
||||
struct Segment {
|
||||
|
||||
int points[2];
|
||||
};
|
||||
|
||||
Vector<Segment> segments;
|
||||
Vector<Point2> points;
|
||||
|
||||
struct BVH {
|
||||
|
||||
Rect2 aabb;
|
||||
int left,right;
|
||||
};
|
||||
|
||||
|
||||
Vector<BVH> bvh;
|
||||
int bvh_depth;
|
||||
|
||||
|
||||
struct BVH_CompareX {
|
||||
|
||||
_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
|
||||
|
||||
return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
|
||||
}
|
||||
};
|
||||
|
||||
struct BVH_CompareY {
|
||||
|
||||
_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
|
||||
|
||||
return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
|
||||
}
|
||||
};
|
||||
|
||||
int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
|
||||
|
||||
public:
|
||||
|
||||
virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
|
||||
|
||||
virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
|
||||
virtual void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
|
||||
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 { return 0; }
|
||||
|
||||
virtual void set_data(const Variant& p_data);
|
||||
virtual Variant get_data() const;
|
||||
|
||||
virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
|
||||
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // SHAPE_2D_2DSW_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -26,135 +26,136 @@
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
#ifndef SPACE_2D_SW_H
|
||||
#define SPACE_2D_SW_H
|
||||
|
||||
#include "typedefs.h"
|
||||
#include "hash_map.h"
|
||||
#include "body_2d_sw.h"
|
||||
#include "area_2d_sw.h"
|
||||
#include "body_pair_2d_sw.h"
|
||||
#include "area_pair_2d_sw.h"
|
||||
#include "broad_phase_2d_sw.h"
|
||||
#include "collision_object_2d_sw.h"
|
||||
|
||||
|
||||
class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {
|
||||
|
||||
OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );
|
||||
public:
|
||||
|
||||
Space2DSW *space;
|
||||
|
||||
bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
|
||||
Physics2DDirectSpaceStateSW();
|
||||
};
|
||||
|
||||
|
||||
|
||||
class Space2DSW {
|
||||
|
||||
|
||||
Physics2DDirectSpaceStateSW *direct_access;
|
||||
RID self;
|
||||
|
||||
BroadPhase2DSW *broadphase;
|
||||
SelfList<Body2DSW>::List active_list;
|
||||
SelfList<Body2DSW>::List inertia_update_list;
|
||||
SelfList<Body2DSW>::List state_query_list;
|
||||
SelfList<Area2DSW>::List monitor_query_list;
|
||||
SelfList<Area2DSW>::List area_moved_list;
|
||||
|
||||
static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
|
||||
static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
|
||||
|
||||
Set<CollisionObject2DSW*> objects;
|
||||
|
||||
Area2DSW *area;
|
||||
|
||||
real_t contact_recycle_radius;
|
||||
real_t contact_max_separation;
|
||||
real_t contact_max_allowed_penetration;
|
||||
real_t constraint_bias;
|
||||
|
||||
enum {
|
||||
|
||||
INTERSECTION_QUERY_MAX=2048
|
||||
};
|
||||
|
||||
CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
|
||||
int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
|
||||
|
||||
float body_linear_velocity_sleep_treshold;
|
||||
float body_angular_velocity_sleep_treshold;
|
||||
float body_time_to_sleep;
|
||||
float body_angular_velocity_damp_ratio;
|
||||
|
||||
bool locked;
|
||||
|
||||
friend class Physics2DDirectSpaceStateSW;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const { return self; }
|
||||
|
||||
void set_default_area(Area2DSW *p_area) { area=p_area; }
|
||||
Area2DSW *get_default_area() const { return area; }
|
||||
|
||||
const SelfList<Body2DSW>::List& get_active_body_list() const;
|
||||
void body_add_to_active_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
|
||||
void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
|
||||
void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
|
||||
void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
|
||||
const SelfList<Area2DSW>::List& get_moved_area_list() const;
|
||||
|
||||
|
||||
|
||||
|
||||
void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
|
||||
|
||||
void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
|
||||
void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
|
||||
|
||||
BroadPhase2DSW *get_broadphase();
|
||||
|
||||
void add_object(CollisionObject2DSW *p_object);
|
||||
void remove_object(CollisionObject2DSW *p_object);
|
||||
const Set<CollisionObject2DSW*> &get_objects() const;
|
||||
|
||||
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
|
||||
_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
|
||||
_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
|
||||
_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
|
||||
_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
|
||||
_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
|
||||
_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
|
||||
_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
|
||||
|
||||
|
||||
void update();
|
||||
void setup();
|
||||
void call_queries();
|
||||
|
||||
|
||||
bool is_locked() const;
|
||||
void lock();
|
||||
void unlock();
|
||||
|
||||
void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
|
||||
real_t get_param(Physics2DServer::SpaceParameter p_param) const;
|
||||
|
||||
Physics2DDirectSpaceStateSW *get_direct_state();
|
||||
|
||||
Space2DSW();
|
||||
~Space2DSW();
|
||||
};
|
||||
|
||||
|
||||
#endif // SPACE_2D_SW_H
|
||||
#ifndef SPACE_2D_SW_H
|
||||
#define SPACE_2D_SW_H
|
||||
|
||||
#include "typedefs.h"
|
||||
#include "hash_map.h"
|
||||
#include "body_2d_sw.h"
|
||||
#include "area_2d_sw.h"
|
||||
#include "body_pair_2d_sw.h"
|
||||
#include "area_pair_2d_sw.h"
|
||||
#include "broad_phase_2d_sw.h"
|
||||
#include "collision_object_2d_sw.h"
|
||||
|
||||
|
||||
class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {
|
||||
|
||||
OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );
|
||||
public:
|
||||
|
||||
Space2DSW *space;
|
||||
|
||||
bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
|
||||
Physics2DDirectSpaceStateSW();
|
||||
};
|
||||
|
||||
|
||||
|
||||
class Space2DSW {
|
||||
|
||||
|
||||
Physics2DDirectSpaceStateSW *direct_access;
|
||||
RID self;
|
||||
|
||||
BroadPhase2DSW *broadphase;
|
||||
SelfList<Body2DSW>::List active_list;
|
||||
SelfList<Body2DSW>::List inertia_update_list;
|
||||
SelfList<Body2DSW>::List state_query_list;
|
||||
SelfList<Area2DSW>::List monitor_query_list;
|
||||
SelfList<Area2DSW>::List area_moved_list;
|
||||
|
||||
static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
|
||||
static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
|
||||
|
||||
Set<CollisionObject2DSW*> objects;
|
||||
|
||||
Area2DSW *area;
|
||||
|
||||
real_t contact_recycle_radius;
|
||||
real_t contact_max_separation;
|
||||
real_t contact_max_allowed_penetration;
|
||||
real_t constraint_bias;
|
||||
|
||||
enum {
|
||||
|
||||
INTERSECTION_QUERY_MAX=2048
|
||||
};
|
||||
|
||||
CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
|
||||
int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
|
||||
|
||||
float body_linear_velocity_sleep_treshold;
|
||||
float body_angular_velocity_sleep_treshold;
|
||||
float body_time_to_sleep;
|
||||
float body_angular_velocity_damp_ratio;
|
||||
|
||||
bool locked;
|
||||
|
||||
friend class Physics2DDirectSpaceStateSW;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const { return self; }
|
||||
|
||||
void set_default_area(Area2DSW *p_area) { area=p_area; }
|
||||
Area2DSW *get_default_area() const { return area; }
|
||||
|
||||
const SelfList<Body2DSW>::List& get_active_body_list() const;
|
||||
void body_add_to_active_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
|
||||
void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
|
||||
void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
|
||||
void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
|
||||
const SelfList<Area2DSW>::List& get_moved_area_list() const;
|
||||
|
||||
|
||||
|
||||
|
||||
void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
|
||||
void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
|
||||
|
||||
void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
|
||||
void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
|
||||
|
||||
BroadPhase2DSW *get_broadphase();
|
||||
|
||||
void add_object(CollisionObject2DSW *p_object);
|
||||
void remove_object(CollisionObject2DSW *p_object);
|
||||
const Set<CollisionObject2DSW*> &get_objects() const;
|
||||
|
||||
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
|
||||
_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
|
||||
_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
|
||||
_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
|
||||
_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
|
||||
_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
|
||||
_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
|
||||
_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
|
||||
|
||||
|
||||
void update();
|
||||
void setup();
|
||||
void call_queries();
|
||||
|
||||
|
||||
bool is_locked() const;
|
||||
void lock();
|
||||
void unlock();
|
||||
|
||||
void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
|
||||
real_t get_param(Physics2DServer::SpaceParameter p_param) const;
|
||||
|
||||
Physics2DDirectSpaceStateSW *get_direct_state();
|
||||
|
||||
Space2DSW();
|
||||
~Space2DSW();
|
||||
};
|
||||
|
||||
|
||||
#endif // SPACE_2D_SW_H
|
||||
|
||||
@@ -49,7 +49,7 @@ void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2
|
||||
if (i==E->get())
|
||||
continue;
|
||||
Body2DSW *b = c->get_body_ptr()[i];
|
||||
if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
|
||||
if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC)
|
||||
continue; //no go
|
||||
_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
|
||||
}
|
||||
@@ -87,8 +87,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
|
||||
Body2DSW *b = p_island;
|
||||
while(b) {
|
||||
|
||||
if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
|
||||
if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
b=b->get_island_next();
|
||||
continue; //ignore for static
|
||||
}
|
||||
|
||||
if (!b->sleep_test(p_delta))
|
||||
can_sleep=false;
|
||||
@@ -101,8 +103,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
|
||||
b = p_island;
|
||||
while(b) {
|
||||
|
||||
if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
|
||||
if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
|
||||
b=b->get_island_next();
|
||||
continue; //ignore for static
|
||||
}
|
||||
|
||||
bool active = b->is_active();
|
||||
|
||||
@@ -210,8 +214,9 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
|
||||
b = body_list->first();
|
||||
while(b) {
|
||||
|
||||
const SelfList<Body2DSW>*n=b->next();
|
||||
b->self()->integrate_velocities(p_delta);
|
||||
b=b->next();
|
||||
b=n; // in case it shuts itself down
|
||||
}
|
||||
|
||||
/* SLEEP / WAKE UP ISLANDS */
|
||||
|
||||
@@ -122,7 +122,7 @@ Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const V
|
||||
if (!res)
|
||||
return Variant();
|
||||
|
||||
Dictionary d;
|
||||
Dictionary d(true);
|
||||
d["position"]=inters.position;
|
||||
d["normal"]=inters.normal;
|
||||
d["collider_id"]=inters.collider_id;
|
||||
@@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
|
||||
|
||||
ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
|
||||
|
||||
int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
|
||||
int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask);
|
||||
|
||||
if (rc==0)
|
||||
return Variant();
|
||||
@@ -160,6 +160,34 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
|
||||
}
|
||||
|
||||
|
||||
Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
|
||||
|
||||
Set<RID> exclude;
|
||||
for(int i=0;i<p_exclude.size();i++)
|
||||
exclude.insert(p_exclude[i]);
|
||||
|
||||
|
||||
|
||||
MotionCastCollision mcc;
|
||||
|
||||
bool result = cast_motion(p_shape,p_xform,p_motion,mcc,exclude,p_user_mask);
|
||||
|
||||
if (!result)
|
||||
return Variant();
|
||||
|
||||
Dictionary d(true);
|
||||
d["point"]=mcc.point;
|
||||
d["normal"]=mcc.normal;
|
||||
d["rid"]=mcc.rid;
|
||||
d["collider_id"]=mcc.collider_id;
|
||||
d["collider"]=mcc.collider;
|
||||
d["shape"]=mcc.shape;
|
||||
|
||||
return d;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -175,6 +203,7 @@ void Physics2DDirectSpaceState::_bind_methods() {
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
|
||||
ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
|
||||
ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
|
||||
|
||||
}
|
||||
|
||||
@@ -297,8 +326,8 @@ void Physics2DServer::_bind_methods() {
|
||||
ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID);
|
||||
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&Physics2DServer::body_set_enable_continuous_collision_detection);
|
||||
ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&Physics2DServer::body_is_continuous_collision_detection_enabled);
|
||||
ObjectTypeDB::bind_method(_MD("body_set_continuous_collision_detection_mode","body","mode"),&Physics2DServer::body_set_continuous_collision_detection_mode);
|
||||
ObjectTypeDB::bind_method(_MD("body_get_continuous_collision_detection_mode","body"),&Physics2DServer::body_get_continuous_collision_detection_mode);
|
||||
|
||||
|
||||
//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&Physics2DServer::body_set_shape,DEFVAL(Matrix32));
|
||||
@@ -307,8 +336,6 @@ void Physics2DServer::_bind_methods() {
|
||||
ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&Physics2DServer::body_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&Physics2DServer::body_get_param);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&Physics2DServer::body_static_simulate_motion);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&Physics2DServer::body_set_state);
|
||||
ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&Physics2DServer::body_get_state);
|
||||
|
||||
@@ -371,7 +398,7 @@ void Physics2DServer::_bind_methods() {
|
||||
BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
|
||||
|
||||
BIND_CONSTANT( BODY_MODE_STATIC );
|
||||
BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
|
||||
BIND_CONSTANT( BODY_MODE_KINEMATIC );
|
||||
BIND_CONSTANT( BODY_MODE_RIGID );
|
||||
BIND_CONSTANT( BODY_MODE_CHARACTER );
|
||||
|
||||
@@ -394,6 +421,10 @@ void Physics2DServer::_bind_methods() {
|
||||
BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
|
||||
BIND_CONSTANT( DAMPED_STRING_DAMPING );
|
||||
|
||||
BIND_CONSTANT( CCD_MODE_DISABLED );
|
||||
BIND_CONSTANT( CCD_MODE_CAST_RAY );
|
||||
BIND_CONSTANT( CCD_MODE_CAST_SHAPE );
|
||||
|
||||
// BIND_CONSTANT( TYPE_BODY );
|
||||
// BIND_CONSTANT( TYPE_AREA );
|
||||
|
||||
|
||||
@@ -90,6 +90,7 @@ class Physics2DDirectSpaceState : public Object {
|
||||
|
||||
Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
|
||||
|
||||
protected:
|
||||
@@ -118,7 +119,26 @@ public:
|
||||
|
||||
};
|
||||
|
||||
virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
|
||||
|
||||
|
||||
struct MotionCastCollision {
|
||||
|
||||
float travel; //0 to 1, if 0 then it's blocked
|
||||
Vector2 point;
|
||||
Vector2 normal;
|
||||
RID rid;
|
||||
ObjectID collider_id;
|
||||
Object *collider;
|
||||
int shape;
|
||||
|
||||
};
|
||||
|
||||
virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
|
||||
|
||||
|
||||
|
||||
Physics2DDirectSpaceState();
|
||||
};
|
||||
@@ -179,6 +199,8 @@ public:
|
||||
virtual Variant shape_get_data(RID p_shape) const=0;
|
||||
virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
|
||||
|
||||
//these work well, but should be used from the main thread only
|
||||
virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
|
||||
|
||||
/* SPACE API */
|
||||
|
||||
@@ -265,10 +287,10 @@ public:
|
||||
|
||||
enum BodyMode {
|
||||
BODY_MODE_STATIC,
|
||||
BODY_MODE_STATIC_ACTIVE,
|
||||
BODY_MODE_KINEMATIC,
|
||||
BODY_MODE_RIGID,
|
||||
//BODY_MODE_SOFT
|
||||
BODY_MODE_CHARACTER
|
||||
//BODY_MODE_SOFT ??
|
||||
};
|
||||
|
||||
virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
|
||||
@@ -277,7 +299,7 @@ public:
|
||||
virtual RID body_get_space(RID p_body) const=0;
|
||||
|
||||
virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
|
||||
virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const=0;
|
||||
virtual BodyMode body_get_mode(RID p_body) const=0;
|
||||
|
||||
virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
|
||||
virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
|
||||
@@ -296,8 +318,14 @@ public:
|
||||
virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
|
||||
virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
|
||||
|
||||
virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
|
||||
virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
|
||||
enum CCDMode {
|
||||
CCD_MODE_DISABLED,
|
||||
CCD_MODE_CAST_RAY,
|
||||
CCD_MODE_CAST_SHAPE,
|
||||
};
|
||||
|
||||
virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0;
|
||||
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0;
|
||||
|
||||
virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
|
||||
virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
|
||||
@@ -313,8 +341,6 @@ public:
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
|
||||
virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
|
||||
|
||||
//advanced simulation
|
||||
virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform)=0;
|
||||
|
||||
//state
|
||||
enum BodyState {
|
||||
@@ -355,6 +381,8 @@ public:
|
||||
|
||||
virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
|
||||
|
||||
virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
enum JointType {
|
||||
@@ -417,6 +445,7 @@ VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::BodyMode );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::BodyParameter );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::BodyState );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::CCDMode );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::JointParam );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::JointType );
|
||||
VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );
|
||||
|
||||
@@ -374,7 +374,7 @@ void PhysicsServer::_bind_methods() {
|
||||
BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
|
||||
|
||||
BIND_CONSTANT( BODY_MODE_STATIC );
|
||||
BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
|
||||
BIND_CONSTANT( BODY_MODE_KINEMATIC );
|
||||
BIND_CONSTANT( BODY_MODE_RIGID );
|
||||
BIND_CONSTANT( BODY_MODE_CHARACTER );
|
||||
|
||||
|
||||
@@ -268,7 +268,7 @@ public:
|
||||
|
||||
enum BodyMode {
|
||||
BODY_MODE_STATIC,
|
||||
BODY_MODE_STATIC_ACTIVE,
|
||||
BODY_MODE_KINEMATIC,
|
||||
BODY_MODE_RIGID,
|
||||
//BODY_MODE_SOFT
|
||||
BODY_MODE_CHARACTER
|
||||
|
||||
Reference in New Issue
Block a user