bool KinematicBody::can_teleport_to(const Vector3 &p_position) {
ERR_FAIL_COND_V(!is_inside_tree(), 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;
Transform xform = get_global_transform();
xform.origin = p_position;
Set<RID> exclude;
exclude.insert(get_rid());
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i), 0, NULL, 1, exclude, get_collision_layer(), mask);
if (col)
return false;
}
return true;
}
bool CollisionObject2D::_get(const StringName &p_name, Variant &r_ret) const {
String name = p_name;
if (name.begins_with("shapes/")) {
int idx = name.get_slicec('/', 1).to_int();
String what = name.get_slicec('/', 2);
if (what == "shape")
r_ret = get_shape(idx);
else if (what == "transform")
r_ret = get_shape_transform(idx);
else if (what == "trigger")
r_ret = is_shape_set_as_trigger(idx);
} else
return false;
return true;
}
bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
//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..
Rect2 body_aabb;
for(int i=0; i<p_body->get_shape_count(); i++) {
if (i==0)
body_aabb=p_body->get_shape_aabb(i);
else
body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
}
body_aabb=body_aabb.grow(p_margin);
Matrix32 body_transform = p_body->get_transform();
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32;
int recover_attempts=4;
Vector2 sr[max_results*2];
do {
Physics2DServerSW::CollCbkData cbk;
cbk.max=max_results;
cbk.amount=0;
cbk.ptr=sr;
CollisionSolver2DSW::CallbackResult cbkres=NULL;
Physics2DServerSW::CollCbkData *cbkptr=NULL;
cbkptr=&cbk;
cbkres=Physics2DServerSW::_shape_col_cbk;
bool collided=false;
int amount = _cull_aabb_for_body(p_body,body_aabb);
for(int j=0; j<p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_trigger(j))
continue;
Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
for(int i=0; i<amount; i++) {
const CollisionObject2DSW *col_obj=intersection_query_results[i];
int shape_idx=intersection_query_subindex_results[i];
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
Vector2 cdir = body->get_one_way_collision_direction();
//if (cdir!=Vector2() && p_motion.dot(cdir)<0)
// continue;
cbk.valid_dir=cdir;
cbk.valid_depth=body->get_one_way_collision_max_depth();
} else {
cbk.valid_dir=Vector2();
cbk.valid_depth=0;
}
if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
collided=cbk.amount>0;
}
}
}
if (!collided)
break;
Vector2 recover_motion;
for(int i=0; i<cbk.amount; 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.4;
}
if (recover_motion==Vector2()) {
collided=false;
break;
}
body_transform.elements[2]+=recover_motion;
body_aabb.pos+=recover_motion;
recover_attempts--;
} while (recover_attempts);
}
float safe = 1.0;
float unsafe = 1.0;
int best_shape=-1;
{
// STEP 2 ATTEMPT MOTION
Rect2 motion_aabb=body_aabb;
motion_aabb.pos+=p_motion;
motion_aabb=motion_aabb.merge(body_aabb);
int amount = _cull_aabb_for_body(p_body,motion_aabb);
for(int j=0; j<p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_trigger(j))
continue;
Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
bool stuck=false;
float best_safe=1;
float best_unsafe=1;
for(int i=0; i<amount; i++) {
const CollisionObject2DSW *col_obj=intersection_query_results[i];
int shape_idx=intersection_query_subindex_results[i];
Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
continue;
}
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
//if one way collision direction ignore initial overlap
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2()) {
continue;
}
}
stuck=true;
break;
}
//just do kinematic solving
float low=0;
float hi=1;
Vector2 mnormal=p_motion.normalized();
for(int i=0; i<8; i++) { //steps should be customizable..
//Matrix32 xfa = p_xform;
float ofs = (low+hi)*0.5;
Vector2 sep=mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0);
if (collided) {
hi=ofs;
} else {
low=ofs;
}
}
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2()) {
Vector2 cd[2];
Physics2DServerSW::CollCbkData cbk;
cbk.max=1;
cbk.amount=0;
cbk.ptr=cd;
cbk.valid_dir=body->get_one_way_collision_direction();
cbk.valid_depth=body->get_one_way_collision_max_depth();
Vector2 sep=mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0);
if (!collided || cbk.amount==0) {
continue;
}
}
}
if (low<best_safe) {
best_safe=low;
best_unsafe=hi;
}
}
if (stuck) {
safe=0;
unsafe=0;
best_shape=j; //sadly it's the best
break;
}
if (best_safe==1.0) {
continue;
}
if (best_safe < safe) {
safe=best_safe;
unsafe=best_unsafe;
best_shape=j;
}
}
}
bool collided=false;
if (safe>=1) {
//not collided
collided=false;
if (r_result) {
r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
r_result->remainder=Vector2();
}
} else {
//it collided, let's get the rest info in unsafe advance
Matrix32 ugt = body_transform;
ugt.elements[2]+=p_motion*unsafe;
_RestCallbackData2D rcd;
rcd.best_len=0;
rcd.best_object=NULL;
rcd.best_shape=0;
Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
Shape2DSW *body_shape = p_body->get_shape(best_shape);
body_aabb.pos+=p_motion*unsafe;
int amount = _cull_aabb_for_body(p_body,body_aabb);
for(int i=0; i<amount; i++) {
const CollisionObject2DSW *col_obj=intersection_query_results[i];
int shape_idx=intersection_query_subindex_results[i];
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
rcd.valid_dir=body->get_one_way_collision_direction();
rcd.valid_depth=body->get_one_way_collision_max_depth();
} else {
rcd.valid_dir=Vector2();
rcd.valid_depth=0;
}
rcd.object=col_obj;
rcd.shape=shape_idx;
bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
if (!sc)
continue;
}
if (rcd.best_len!=0) {
if (r_result) {
r_result->collider=rcd.best_object->get_self();
r_result->collider_id=rcd.best_object->get_instance_id();
r_result->collider_shape=rcd.best_shape;
r_result->collision_normal=rcd.best_normal;
r_result->collision_point=rcd.best_contact;
r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
r_result->remainder=p_motion - safe * p_motion;
}
collided=true;
} else {
if (r_result) {
r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
r_result->remainder=Vector2();
}
collided=false;
}
}
return collided;
#if 0
//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_tree(),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 {
//motion recover
for(int i=0; i<get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),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.4;
}
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++) {
if (is_shape_set_as_trigger(i))
continue;
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,get_layer_mask(),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,get_layer_mask(),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;
collider_shape=rest_info.shape;
collider_metadata=rest_info.metadata;
}
}
Vector2 motion=p_motion*safe;
Matrix32 gt = get_global_transform();
gt.elements[2]+=motion;
set_global_transform(gt);
return p_motion-motion;
#endif
return false;
}
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_tree(), 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 (is_shape_set_as_trigger(i))
continue;
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), m, sr, max_shapes, res_shapes, exclude, get_collision_layer(), 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++) {
if (is_shape_set_as_trigger(i))
continue;
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_collision_layer(), 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_collision_layer(), 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;
collider_shape = rest.shape;
}
}
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;
}
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_tree(),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 {
//motion recover
for(int i=0;i<get_shape_count();i++) {
if (is_shape_set_as_trigger(i))
continue;
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),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++) {
if (is_shape_set_as_trigger(i))
continue;
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,get_layer_mask(),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,get_layer_mask(),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;
collider_shape=rest_info.shape;
collider_metadata=rest_info.metadata;
}
}
Vector2 motion=p_motion*safe;
Matrix32 gt = get_global_transform();
gt.elements[2]+=motion;
set_global_transform(gt);
return p_motion-motion;
}
