bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, 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..
if (r_result) {
r_result->collider_id=0;
r_result->collider_shape=0;
}
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_from;
{
//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];
#if 0
Vector2 rel = b-a;
float d = rel.length();
if (d==0)
continue;
Vector2 n = rel/d;
float traveled = n.dot(recover_motion);
a+=n*traveled;
#endif
// 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..
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;
r_result->remainder=Vector2();
r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
}
} 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;
r_result->remainder=p_motion - safe * p_motion;
r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
}
collided=true;
} else {
if (r_result) {
r_result->motion=p_motion;
r_result->remainder=Vector2();
r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
}
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;
}
void BodySW::update_inertias() {
//update shapes and motions
switch (mode) {
case PhysicsServer::BODY_MODE_RIGID: {
//update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
real_t total_area = 0;
for (int i = 0; i < get_shape_count(); i++) {
total_area += get_shape_area(i);
}
// We have to recompute the center of mass
center_of_mass_local.zero();
for (int i = 0; i < get_shape_count(); i++) {
real_t area = get_shape_area(i);
real_t mass = area * this->mass / total_area;
// NOTE: we assume that the shape origin is also its center of mass
center_of_mass_local += mass * get_shape_transform(i).origin;
}
center_of_mass_local /= mass;
// Recompute the inertia tensor
Basis inertia_tensor;
inertia_tensor.set_zero();
for (int i = 0; i < get_shape_count(); i++) {
const ShapeSW *shape = get_shape(i);
real_t area = get_shape_area(i);
real_t mass = area * this->mass / total_area;
Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
Transform shape_transform = get_shape_transform(i);
Basis shape_basis = shape_transform.basis.orthonormalized();
// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
}
// Compute the principal axes of inertia
principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
if (mass)
_inv_mass = 1.0 / mass;
else
_inv_mass = 0;
} break;
case PhysicsServer::BODY_MODE_KINEMATIC:
case PhysicsServer::BODY_MODE_STATIC: {
_inv_inertia_tensor.set_zero();
_inv_mass = 0;
} break;
case PhysicsServer::BODY_MODE_CHARACTER: {
_inv_inertia_tensor.set_zero();
_inv_mass = 1.0 / mass;
} break;
}
//_update_shapes();
_update_transform_dependant();
}
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 {
//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),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++) {
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;
}
}
Vector2 motion=p_motion*safe;
Matrix32 gt = get_global_transform();
gt.elements[2]+=motion;
set_global_transform(gt);
return p_motion-motion;
}
