void RigidBody2D::set_use_custom_integrator(bool p_enable){
if (custom_integrator==p_enable)
return;
custom_integrator=p_enable;
Physics2DServer::get_singleton()->body_set_omit_force_integration(get_rid(),p_enable);
}
void PhysicsBody::add_collision_exception_with(Node* p_node) {
ERR_FAIL_NULL(p_node);
PhysicsBody *physics_body = p_node->cast_to<PhysicsBody>();
if (!physics_body) {
ERR_EXPLAIN("Collision exception only works between two objects of PhysicsBody type");
}
ERR_FAIL_COND(!physics_body);
PhysicsServer::get_singleton()->body_add_collision_exception(get_rid(),physics_body->get_rid());
}
void RigidBody2D::set_linear_velocity(const Vector2& p_velocity){
linear_velocity=p_velocity;
if (state)
state->set_linear_velocity(linear_velocity);
else {
Physics2DServer::get_singleton()->body_set_state(get_rid(),Physics2DServer::BODY_STATE_LINEAR_VELOCITY,linear_velocity);
}
}
void StaticBody2D::_update_xform() {
if (!pre_xform || !pending)
return;
setting=true;
Matrix32 new_xform = get_global_transform(); //obtain the new one
set_block_transform_notify(true);
Physics2DServer::get_singleton()->body_set_state(get_rid(),Physics2DServer::BODY_STATE_TRANSFORM,*pre_xform); //then simulate motion!
set_global_transform(*pre_xform); //but restore state to previous one in both visual and physics
set_block_transform_notify(false);
Physics2DServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion!
setting=false;
pending=false;
}
VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
m_pitchControl=0;
m_currentVehicleSpeedKmHour = real_t(0.);
m_steeringValue = real_t(0.);
engine_force=0;
brake=0;
friction=1;
ccd=false;
exclude.insert(get_rid());
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
set_mass(40);
}
void Area2D::set_enable_monitoring(bool p_enable) {
if (locked) {
ERR_EXPLAIN("This function can't be used during the in/out signal.");
}
ERR_FAIL_COND(locked);
if (p_enable==monitoring)
return;
monitoring=p_enable;
if (monitoring) {
Physics2DServer::get_singleton()->area_set_monitor_callback(get_rid(),this,"_body_inout");
} else {
Physics2DServer::get_singleton()->area_set_monitor_callback(get_rid(),NULL,StringName());
_clear_monitoring();
}
}
void StaticBody::set_simulate_motion(bool p_enable) {
if (p_enable==simulating_motion)
return;
simulating_motion=p_enable;
if (p_enable) {
pre_xform = memnew( Transform );
if (is_inside_scene())
*pre_xform=get_transform();
// query = PhysicsServer::get_singleton()->query_create(this,"_state_notify",Variant());
// PhysicsServer::get_singleton()->query_body_direct_state(query,get_rid());
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_state_notify");
} else {
memdelete( pre_xform );
pre_xform=NULL;
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName());
pending=false;
}
}
void RigidBody::set_axis_velocity(const Vector3& p_axis) {
Vector3 v = state? state->get_linear_velocity() : linear_velocity;
Vector3 axis = p_axis.normalized();
v-=axis*axis.dot(v);
v+=p_axis;
if (state) {
set_linear_velocity(v);
} else {
PhysicsServer::get_singleton()->body_set_axis_velocity(get_rid(),p_axis);
linear_velocity=v;
}
}
void Area::set_monitoring(bool p_enable) {
if (locked) {
ERR_EXPLAIN("Function blocked during in/out signal. Use set_deferred(\"monitoring\",true/false)");
}
ERR_FAIL_COND(locked);
if (p_enable == monitoring)
return;
monitoring = p_enable;
if (monitoring) {
PhysicsServer::get_singleton()->area_set_monitor_callback(get_rid(), this, SceneStringNames::get_singleton()->_body_inout);
PhysicsServer::get_singleton()->area_set_area_monitor_callback(get_rid(), this, SceneStringNames::get_singleton()->_area_inout);
} else {
PhysicsServer::get_singleton()->area_set_monitor_callback(get_rid(), NULL, StringName());
PhysicsServer::get_singleton()->area_set_area_monitor_callback(get_rid(), NULL, StringName());
_clear_monitoring();
}
}
void Area::set_monitorable(bool p_enable) {
if (locked) {
ERR_EXPLAIN("This function can't be used during the in/out signal.");
}
ERR_FAIL_COND(locked);
if (p_enable==monitorable)
return;
monitorable=p_enable;
PhysicsServer::get_singleton()->area_set_monitorable(get_rid(),monitorable);
}
void Area::set_monitorable(bool p_enable) {
if (locked || PhysicsServer::get_singleton()->is_flushing_queries()) {
ERR_EXPLAIN("Function blocked during in/out signal. Use set_deferred(\"monitorable\",true/false)");
}
ERR_FAIL_COND(locked || PhysicsServer::get_singleton()->is_flushing_queries());
if (p_enable == monitorable)
return;
monitorable = p_enable;
PhysicsServer::get_singleton()->area_set_monitorable(get_rid(), monitorable);
}
void RigidBody::set_mode(Mode p_mode) {
mode = p_mode;
switch (p_mode) {
case MODE_RIGID: {
PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_RIGID);
} break;
case MODE_STATIC: {
PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_STATIC);
} break;
case MODE_CHARACTER: {
PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_CHARACTER);
} break;
case MODE_KINEMATIC: {
PhysicsServer::get_singleton()->body_set_mode(get_rid(), PhysicsServer::BODY_MODE_KINEMATIC);
} break;
}
}
RigidBody2D::RigidBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_RIGID) {
mode=MODE_RIGID;
bounce=0;
mass=1;
friction=1;
max_contacts_reported=0;
state=NULL;
angular_velocity=0;
active=true;
ccd_mode=CCD_MODE_DISABLED;
custom_integrator=false;
contact_monitor=NULL;
can_sleep=true;
Physics2DServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
}
RigidBody::RigidBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
mode=MODE_RIGID;
bounce=0;
mass=1;
friction=1;
max_contacts_reported=0;
state=NULL;
//angular_velocity=0;
sleeping=false;
ccd=false;
custom_integrator=false;
contact_monitor=NULL;
can_sleep=true;
axis_lock = AXIS_LOCK_DISABLED;
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
}
bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
ERR_FAIL_COND_V(!is_inside_scene(),false);
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss,false);
uint32_t mask=0;
if (collide_static)
mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
Vector3 motion = p_position-get_global_transform().origin;
Transform xform=get_global_transform();
if (true || p_discrete) {
xform.origin+=motion;
motion=Vector3();
}
Set<RID> exclude;
exclude.insert(get_rid());
//fill exclude list..
for(int i=0;i<get_shape_count();i++) {
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask);
if (col)
return false;
}
return true;
}
void print_transaction_index(tick_t now, int chan_id, uint64_t tindex){
transaction_t *this_t = NULL;
command_t *this_c;
int rid;
this_t = &(dram_system.dram_controller[chan_id].transaction_queue.entry[tindex]);
rid = get_rid(get_biu_address(),this_t->slot_id);
assert(tindex <= MAX_TRANSACTION_QUEUE_DEPTH);
if(this_t->transaction_type == MEMORY_READ_COMMAND ){
fprintf(stdout,"READ_TRANSACTION [%d] to addr[%llx] rid(%d)\n",this_t->transaction_id,this_t->address.physical_address,rid);
} else if(this_t->transaction_type == MEMORY_IFETCH_COMMAND ){
fprintf(stdout,"IFETCH_TRANSACTION [%d] to addr[%llx] rid(%d)\n",this_t->transaction_id,this_t->address.physical_address,rid);
} else if(this_t->transaction_type == MEMORY_WRITE_COMMAND ){
fprintf(stdout,"WRITE_TRANSACTION [%d] to addr[%llx] rid(%d)\n",this_t->transaction_id,this_t->address.physical_address,rid );
} else if(this_t->transaction_type == MEMORY_PREFETCH ){
fprintf(stdout,"MEMORY_PREFETCH [%d] to addr[%llx] rid(%d)\n",this_t->transaction_id,this_t->address.physical_address,rid);
}
this_c = this_t->next_c;
while(this_c != NULL){
print_command(now, this_c);
this_c = this_c->next_c;
}
fprintf(stdout,"------------------\n");
fflush(stdout);
}
Vector2 KinematicBody2D::move(const Vector2& p_motion) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
colliding=false;
ERR_FAIL_COND_V(!is_inside_scene(),Vector2());
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
ERR_FAIL_COND_V(!dss,Vector2());
const int max_shapes=32;
Vector2 sr[max_shapes*2];
int res_shapes;
Set<RID> exclude;
exclude.insert(get_rid());
//recover first
int recover_attempts=4;
bool collided=false;
uint32_t mask=0;
if (collide_static)
mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
// print_line("motion: "+p_motion+" margin: "+rtos(margin));
//print_line("margin: "+rtos(margin));
do {
//fill exclude list..
for(int i=0;i<get_shape_count();i++) {
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,0,mask))
collided=true;
}
if (!collided)
break;
Vector2 recover_motion;
for(int i=0;i<res_shapes;i++) {
Vector2 a = sr[i*2+0];
Vector2 b = sr[i*2+1];
float d = a.distance_to(b);
//if (d<margin)
/// continue;
recover_motion+=(b-a)*0.2;
}
if (recover_motion==Vector2()) {
collided=false;
break;
}
Matrix32 gt = get_global_transform();
gt.elements[2]+=recover_motion;
set_global_transform(gt);
recover_attempts--;
} while (recover_attempts);
//move second
float safe = 1.0;
float unsafe = 1.0;
int best_shape=-1;
for(int i=0;i<get_shape_count();i++) {
float lsafe,lunsafe;
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,0,mask);
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
if (!valid) {
safe=0;
unsafe=0;
best_shape=i; //sadly it's the best
break;
}
if (lsafe==1.0) {
continue;
}
if (lsafe < safe) {
safe=lsafe;
unsafe=lunsafe;
best_shape=i;
}
}
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
if (safe>=1) {
//not collided
colliding=false;
} else {
//it collided, let's get the rest info in unsafe advance
Matrix32 ugt = get_global_transform();
ugt.elements[2]+=p_motion*unsafe;
Physics2DDirectSpaceState::ShapeRestInfo rest_info;
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,0,mask);
if (!c2) {
//should not happen, but floating point precision is so weird..
colliding=false;
} else {
//print_line("Travel: "+rtos(travel));
colliding=true;
collision=rest_info.point;
normal=rest_info.normal;
collider=rest_info.collider_id;
collider_vel=rest_info.linear_velocity;
}
}
Vector2 motion=p_motion*safe;
Matrix32 gt = get_global_transform();
gt.elements[2]+=motion;
set_global_transform(gt);
return p_motion-motion;
}
void RigidBody2D::set_applied_force(const Vector2& p_force) {
Physics2DServer::get_singleton()->body_set_applied_force(get_rid(), p_force);
};
Vector3 KinematicBody::move(const Vector3& p_motion) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
colliding=false;
ERR_FAIL_COND_V(!is_inside_scene(),Vector3());
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss,Vector3());
const int max_shapes=32;
Vector3 sr[max_shapes*2];
int res_shapes;
Set<RID> exclude;
exclude.insert(get_rid());
//recover first
int recover_attempts=4;
bool collided=false;
uint32_t mask=0;
if (collide_static)
mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
// print_line("motion: "+p_motion+" margin: "+rtos(margin));
//print_line("margin: "+rtos(margin));
float m = margin;
//m=0.001;
do {
//motion recover
for(int i=0;i<get_shape_count();i++) {
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),m,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) {
collided=true;
}
}
if (!collided)
break;
//print_line("have to recover");
Vector3 recover_motion;
bool all_outside=true;
for(int j=0;j<8;j++) {
for(int i=0;i<res_shapes;i++) {
Vector3 a = sr[i*2+0];
Vector3 b = sr[i*2+1];
//print_line(String()+a+" -> "+b);
#if 0
float d = a.distance_to(b);
//if (d<margin)
/// continue;
///
///
recover_motion+=(b-a)*0.2;
#else
float dist = a.distance_to(b);
if (dist>CMP_EPSILON) {
Vector3 norm = (b-a).normalized();
if (dist>margin*0.5)
all_outside=false;
float adv = norm.dot(recover_motion);
//print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv));
recover_motion+=norm*MAX(dist-adv,0)*0.4;
}
#endif
}
}
if (recover_motion==Vector3()) {
collided=false;
break;
}
//print_line("**** RECOVER: "+recover_motion);
Transform gt = get_global_transform();
gt.origin+=recover_motion;
set_global_transform(gt);
recover_attempts--;
if (all_outside)
break;
} while (recover_attempts);
//move second
float safe = 1.0;
float unsafe = 1.0;
int best_shape=-1;
PhysicsDirectSpaceState::ShapeRestInfo rest;
//print_line("pos: "+get_global_transform().origin);
//print_line("motion: "+p_motion);
for(int i=0;i<get_shape_count();i++) {
float lsafe,lunsafe;
PhysicsDirectSpaceState::ShapeRestInfo lrest;
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion,0, lsafe,lunsafe,exclude,get_layer_mask(),mask,&lrest);
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
if (!valid) {
safe=0;
unsafe=0;
best_shape=i; //sadly it's the best
//print_line("initial stuck");
break;
}
if (lsafe==1.0) {
//print_line("initial free");
continue;
}
if (lsafe < safe) {
//print_line("initial at "+rtos(lsafe));
safe=lsafe;
safe=MAX(0,lsafe-0.01);
unsafe=lunsafe;
best_shape=i;
rest=lrest;
}
}
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
if (safe>=1) {
//not collided
colliding=false;
} else {
colliding=true;
if (true || (safe==0 && unsafe==0)) { //use it always because it's more precise than GJK
//no advance, use rest info from collision
Transform ugt = get_global_transform();
ugt.origin+=p_motion*unsafe;
PhysicsDirectSpaceState::ShapeRestInfo rest_info;
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), m,&rest,exclude,get_layer_mask(),mask);
if (!c2) {
//should not happen, but floating point precision is so weird..
colliding=false;
}
// print_line("Rest Travel: "+rest.normal);
}
if (colliding) {
collision=rest.point;
normal=rest.normal;
collider=rest.collider_id;
collider_vel=rest.linear_velocity;
}
}
Vector3 motion=p_motion*safe;
//if (colliding)
// motion+=normal*0.001;
Transform gt = get_global_transform();
gt.origin+=motion;
set_global_transform(gt);
return p_motion-motion;
}
void RigidBody::set_axis_lock(AxisLock p_lock) {
axis_lock=p_lock;
PhysicsServer::get_singleton()->body_set_axis_lock(get_rid(),PhysicsServer::BodyAxisLock(axis_lock));
}
Vector2 RigidBody2D::get_applied_force() const {
return Physics2DServer::get_singleton()->body_get_applied_force(get_rid());
};
void RigidBody::set_use_continuous_collision_detection(bool p_enable) {
ccd=p_enable;
PhysicsServer::get_singleton()->body_set_enable_continuous_collision_detection(get_rid(),p_enable);
}
void PhysicsBody::set_layer_mask(uint32_t p_mask) {
layer_mask=p_mask;
PhysicsServer::get_singleton()->body_set_layer_mask(get_rid(),p_mask);
}
void RigidBody::set_max_contacts_reported(int p_amount) {
max_contacts_reported=p_amount;
PhysicsServer::get_singleton()->body_set_max_contacts_reported(get_rid(),p_amount);
}
void RigidBody::apply_impulse(const Vector3& p_pos, const Vector3& p_impulse) {
PhysicsServer::get_singleton()->body_apply_impulse(get_rid(),p_pos,p_impulse);
}
void RigidBody::set_can_sleep(bool p_active) {
can_sleep=p_active;
PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_CAN_SLEEP,p_active);
}
void RigidBody::set_sleeping(bool p_sleeping) {
sleeping=p_sleeping;
PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_SLEEPING,sleeping);
}
void RigidBody2D::set_continuous_collision_detection_mode(CCDMode p_mode) {
ccd_mode=p_mode;
Physics2DServer::get_singleton()->body_set_continuous_collision_detection_mode(get_rid(),Physics2DServer::CCDMode(p_mode));
}
void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) {
constant_angular_velocity=p_vel;
PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity);
}
void CircleShape2D::_update_shape() {
Physics2DServer::get_singleton()->shape_set_data(get_rid(), radius);
emit_changed();
}
