void Spatial::look_at_from_pos(const Vector3 &p_pos, const Vector3 &p_target, const Vector3 &p_up_normal) {
Transform lookat;
lookat.origin = p_pos;
lookat = lookat.looking_at(p_target, p_up_normal);
set_global_transform(lookat);
}
void Camera::look_at(const Vector3& p_target, const Vector3& p_up_normal) {
Transform lookat;
lookat.origin=get_global_transform().origin;
lookat=lookat.looking_at(p_target,p_up_normal);
set_global_transform(lookat);
}
void Spatial::global_rotate(const Vector3 &p_normal, float p_radians) {
Matrix3 rotation(p_normal, p_radians);
Transform t = get_global_transform();
t.basis = rotation * t.basis;
set_global_transform(t);
}
void InterpolatedCamera::_notification(int p_what) {
switch(p_what) {
case NOTIFICATION_ENTER_SCENE: {
if (get_scene()->is_editor_hint() && enabled)
set_fixed_process(false);
} break;
case NOTIFICATION_PROCESS: {
if (!enabled)
break;
if (has_node(target)) {
Spatial *node = get_node(target)->cast_to<Spatial>();
if (!node)
break;
float delta = speed*get_process_delta_time();
Transform target_xform = node->get_global_transform();
Transform local_transform = get_transform();
local_transform = local_transform.interpolate_with(target_xform,delta);
set_global_transform(local_transform);
if (node->cast_to<Camera>()) {
Camera *cam = node->cast_to<Camera>();
if (cam->get_projection()==get_projection()) {
float new_near = Math::lerp(get_znear(),cam->get_znear(),delta);
float new_far = Math::lerp(get_zfar(),cam->get_zfar(),delta);
if (cam->get_projection()==PROJECTION_ORTHOGONAL) {
float size = Math::lerp(get_size(),cam->get_size(),delta);
set_orthogonal(size,new_near,new_far);
} else {
float fov = Math::lerp(get_fov(),cam->get_fov(),delta);
set_perspective(fov,new_near,new_far);
}
}
}
}
} break;
}
}
void Spatial::look_at(const Vector3 &p_target, const Vector3 &p_up_normal) {
Transform lookat;
lookat.origin = get_global_transform().origin;
if (lookat.origin == p_target) {
ERR_EXPLAIN("Node origin and target are in the same position, look_at() failed");
ERR_FAIL();
}
if (p_up_normal.cross(p_target - lookat.origin) == Vector3()) {
ERR_EXPLAIN("Up vector and direction between node origin and target are aligned, look_at() failed");
ERR_FAIL();
}
lookat = lookat.looking_at(p_target, p_up_normal);
set_global_transform(lookat);
}
void StaticBody::_state_notify(Object *p_object) {
if (!pre_xform)
return;
PhysicsDirectBodyState *p2d = (PhysicsDirectBodyState*)p_object;
setting=true;
Transform new_xform = p2d->get_transform();
*pre_xform=new_xform;
set_ignore_transform_notification(true);
set_global_transform(new_xform);
set_ignore_transform_notification(false);
setting=false;
}
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;
}
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::_direct_state_changed(Object *p_state) {
//eh.. f**k
#ifdef DEBUG_ENABLED
state=p_state->cast_to<PhysicsDirectBodyState>();
#else
state=(PhysicsDirectBodyState*)p_state; //trust it
#endif
if (contact_monitor) {
//untag all
int rc=0;
for( Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.front();E;E=E->next()) {
for(int i=0;i<E->get().shapes.size();i++) {
E->get().shapes[i].tagged=false;
rc++;
}
}
_RigidBodyInOut *toadd=(_RigidBodyInOut*)alloca(state->get_contact_count()*sizeof(_RigidBodyInOut));
int toadd_count=0;//state->get_contact_count();
RigidBody_RemoveAction *toremove=(RigidBody_RemoveAction*)alloca(rc*sizeof(RigidBody_RemoveAction));
int toremove_count=0;
//put the ones to add
for(int i=0;i<state->get_contact_count();i++) {
ObjectID obj = state->get_contact_collider_id(i);
int local_shape = state->get_contact_local_shape(i);
int shape = state->get_contact_collider_shape(i);
toadd[i].local_shape=local_shape;
toadd[i].id=obj;
toadd[i].shape=shape;
bool found=false;
Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.find(obj);
if (!E) {
toadd_count++;
continue;
}
ShapePair sp( shape,local_shape );
int idx = E->get().shapes.find(sp);
if (idx==-1) {
toadd_count++;
continue;
}
E->get().shapes[idx].tagged=true;
}
//put the ones to remove
for( Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.front();E;E=E->next()) {
for(int i=0;i<E->get().shapes.size();i++) {
if (!E->get().shapes[i].tagged) {
toremove[toremove_count].body_id=E->key();
toremove[toremove_count].pair=E->get().shapes[i];
toremove_count++;
}
}
}
//process remotions
for(int i=0;i<toremove_count;i++) {
_body_inout(0,toremove[i].body_id,toremove[i].pair.body_shape,toremove[i].pair.local_shape);
}
//process aditions
for(int i=0;i<toadd_count;i++) {
_body_inout(1,toadd[i].id,toadd[i].shape,toadd[i].local_shape);
}
}
set_ignore_transform_notification(true);
set_global_transform(state->get_transform());
linear_velocity=state->get_linear_velocity();
angular_velocity=state->get_angular_velocity();
sleeping=state->is_sleeping();
if (get_script_instance())
get_script_instance()->call("_integrate_forces",state);
set_ignore_transform_notification(false);
state=NULL;
}
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;
}
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;
}
void Spatial::global_translate(const Vector3 &p_offset) {
Transform t = get_global_transform();
t.origin += p_offset;
set_global_transform(t);
}
void VehicleBody::_direct_state_changed(Object *p_state) {
PhysicsDirectBodyState *s = p_state->cast_to<PhysicsDirectBodyState>();
set_ignore_transform_notification(true);
set_global_transform(s->get_transform());
set_ignore_transform_notification(false);
float step = s->get_step();
for(int i=0;i<wheels.size();i++) {
_update_wheel(i,s);
}
for(int i=0;i<wheels.size();i++) {
_ray_cast(i,s);
wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform);
}
_update_suspension(s);
for(int i=0;i<wheels.size();i++) {
//apply suspension force
VehicleWheel& wheel = *wheels[i];
real_t suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin;
s->apply_impulse(relpos,impulse);
//getRigidBody()->applyImpulse(impulse, relpos);
}
_update_friction(s);
for (int i=0;i<wheels.size();i++)
{
VehicleWheel& wheel = *wheels[i];
Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - s->get_transform().origin;
Vector3 vel = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos);// * mPos);
if (wheel.m_raycastInfo.m_isInContact)
{
const Transform& chassisWorldTransform = s->get_transform();
Vector3 fwd (
chassisWorldTransform.basis[0][Vector3::AXIS_Z],
chassisWorldTransform.basis[1][Vector3::AXIS_Z],
chassisWorldTransform.basis[2][Vector3::AXIS_Z]);
real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
real_t proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= real_t(0.99);//damping of rotation when not in contact
}
linear_velocity = s->get_linear_velocity();
}
void Spatial::global_scale(const Vector3 &p_scale) {
Transform t = get_global_transform();
t.basis.scale(p_scale);
set_global_transform(t);
}
void Spatial::global_rotate(const Vector3 &p_axis, float p_angle) {
Transform t = get_global_transform();
t.basis.rotate(p_axis, p_angle);
set_global_transform(t);
}