text: /*************************************************************************/
  /*  body_2d_sw.h                                                         */
  /*************************************************************************/
  /*                       This file is part of:                           */
  /*                           GODOT ENGINE                                */
  /*                    http://www.godotengine.org                         */
  /*************************************************************************/
  /* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur.                 */
  /*                                                                       */
  /* Permission is hereby granted, free of charge, to any person obtaining */
  /* a copy of this software and associated documentation files (the       */
  /* "Software"), to deal in the Software without restriction, including   */
  /* without limitation the rights to use, copy, modify, merge, publish,   */
  /* distribute, sublicense, and/or sell copies of the Software, and to    */
  /* permit persons to whom the Software is furnished to do so, subject to */
  /* the following conditions:                                             */
  /*                                                                       */
  /* The above copyright notice and this permission notice shall be        */
  /* included in all copies or substantial portions of the Software.       */
  /*                                                                       */
  /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
  /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
  /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
  /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
  /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
  /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
  /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
  /*************************************************************************/
  #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 linear_damp;
  	real_t angular_damp;
  	real_t gravity_scale;
  
  	real_t mass;
  	real_t bounce;
  	real_t friction;
  
  	real_t _inv_mass;
  	real_t _inv_inertia;
  
  	Vector2 gravity;
  	real_t area_linear_damp;
  	real_t area_angular_damp;
  
  	real_t still_time;
  
  	Vector2 applied_force;
  	real_t applied_torque;
  
  	Vector2 one_way_collision_direction;
  	float one_way_collision_max_depth;
  
  
  	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;
  	bool first_time_kinematic;
  	bool first_integration;
  	bool using_one_way_cache;
  	void _update_inertia();
  	virtual void _shapes_changed();
  	Matrix32 new_transform;
  
  
  	Map<Constraint2DSW*,int> constraint_map;
  
  	struct AreaCMP {
  
  		Area2DSW *area;
  		int refCount;
  		_FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();}
  		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();}
  		_FORCE_INLINE_ AreaCMP() {}
  		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area; refCount=1;}
  	};
  
  
  	Vector<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_and_dampenings(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) {
  		int index = areas.find(AreaCMP(p_area));
  		if( index > -1 ) {
  			areas[index].refCount += 1;
  		} else {
  			areas.ordered_insert(AreaCMP(p_area));
  		}
  	}
  
  	_FORCE_INLINE_ void remove_area(Area2DSW *p_area) {
  		int index = areas.find(AreaCMP(p_area));
  		if( index > -1 ) {
  			areas[index].refCount -= 1;
  			if( areas[index].refCount < 1 )
  				areas.remove(index);
  		}
  	}
  
  	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
  
  	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
  
  	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
  	_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; }
  
  	_FORCE_INLINE_ void wakeup() {
  		if ((!get_space()) || mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
  			return;
  		set_active(true);
  	}
  
  
  
  
  	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_one_way_collision_direction(const Vector2& p_dir) {
  		one_way_collision_direction=p_dir;
  		using_one_way_cache=one_way_collision_direction!=Vector2();
  	}
  	Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; }
  
  	void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth=p_depth; }
  	float get_one_way_collision_max_depth() const { return one_way_collision_max_depth; }
  
  	_FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; }
  
  	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_bounce() const { return bounce; }
  	_FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
  	_FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; }
  
  
  	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->gravity;  } // get gravity vector working on this body space/area
  	virtual float get_total_angular_damp() const {  return body->area_angular_damp;  } // get density of this body space/area
  	virtual float get_total_linear_damp() const {  return body->area_linear_damp;  } // 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 Variant get_contact_collider_shape_metadata(int p_contact_idx) const;
  
  	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
  