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; }
void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) { ERR_FAIL_INDEX(p_index, get_shape_count()); shapes.write[p_index].set_transform(p_transform); // Note, enableDynamicAabbTree is false because on transform change compound is destroyed reload_shapes(); }
int JB2Image::add_blit(const JB2Blit &blit) { if (blit.shapeno >= (unsigned int)get_shape_count()) G_THROW( ERR_MSG("JB2Image.bad_shape") ); int index = blits.size(); blits.touch(index); blits[index] = blit; return index; }
int JB2Dict::add_shape(const JB2Shape &shape) { if (shape.parent >= get_shape_count()) G_THROW( ERR_MSG("JB2Image.bad_parent_shape") ); int index = shapes.size(); shapes.touch(index); shapes[index] = shape; return index + inherited_shapes; }
void CollisionObject2DSW::_set_static(bool p_static) { if (_static == p_static) return; _static = p_static; if (!space) return; for (int i = 0; i < get_shape_count(); i++) { Shape &s = shapes[i]; if (s.bpid > 0) { space->get_broadphase()->set_static(s.bpid, _static); } } }
BC_PHYSICSIMP_DLL void bc_platform_rigid_actor< g_api_physx >::set_notify_flag(bc_shape_notify_flag p_flag, bool p_value) noexcept { auto l_buffer_size = get_shape_count(); auto* l_buffer = static_cast< bc_shape* > ( bcAlloc(sizeof(bc_shape*) * l_buffer_size, core::bc_alloc_type::frame) ); get_shapes(l_buffer, l_buffer_size); for (bcUINT32 i = 0; i < l_buffer_size; ++i) { l_buffer[i].set_notify_flag(p_flag, p_value); } bcFree(l_buffer); }
BC_PHYSICSIMP_DLL void bc_platform_rigid_actor< g_api_physx >::set_collision_group(bc_collision_filter p_filter) noexcept { auto l_buffer_size = get_shape_count(); auto* l_buffer = static_cast< bc_shape* > ( bcAlloc(sizeof(bc_shape*) * l_buffer_size, core::bc_alloc_type::frame) ); get_shapes(l_buffer, l_buffer_size); for (bcUINT32 i = 0; i < l_buffer_size; ++i) { l_buffer[i].set_collision_group(p_filter); } bcFree(l_buffer); }
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; }
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; }
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 Body2DSW::update_inertias() { //update shapes and motions switch(mode) { case Physics2DServer::BODY_MODE_RIGID: { //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) float total_area=0; for (int i=0;i<get_shape_count();i++) { total_area+=get_shape_aabb(i).get_area(); } real_t _inertia=0; for (int i=0;i<get_shape_count();i++) { const Shape2DSW* shape=get_shape(i); float area=get_shape_aabb(i).get_area(); float mass = area * this->mass / total_area; Matrix32 mtx = get_shape_transform(i); Vector2 scale = mtx.get_scale(); _inertia += shape->get_moment_of_inertia(mass,scale) + mass * mtx.get_origin().length_squared(); //Rect2 ab = get_shape_aabb(i); //_inertia+=mass*ab.size.dot(ab.size)/12.0f; } if (_inertia!=0) _inv_inertia=1.0/_inertia; else _inv_inertia=0.0; //wathever if (mass) _inv_mass=1.0/mass; else _inv_mass=0; } break; case Physics2DServer::BODY_MODE_KINEMATIC: case Physics2DServer::BODY_MODE_STATIC: { _inv_inertia=0; _inv_mass=0; } break; case Physics2DServer::BODY_MODE_CHARACTER: { _inv_inertia=0; _inv_mass=1.0/mass; } break; } //_update_inertia_tensor(); //_update_shapes(); }
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 BodySW::update_inertias() { //update shapes and motions switch(mode) { case PhysicsServer::BODY_MODE_RIGID: { //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) float total_area=0; for (int i=0;i<get_shape_count();i++) { total_area+=get_shape_aabb(i).get_area(); } Vector3 _inertia; for (int i=0;i<get_shape_count();i++) { const ShapeSW* shape=get_shape(i); float area=get_shape_aabb(i).get_area(); float mass = area * this->mass / total_area; _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); } if (_inertia!=Vector3()) _inv_inertia=_inertia.inverse(); else _inv_inertia=Vector3(); if (mass) _inv_mass=1.0/mass; else _inv_mass=0; } break; case PhysicsServer::BODY_MODE_KINEMATIC: case PhysicsServer::BODY_MODE_STATIC: { _inv_inertia=Vector3(); _inv_mass=0; } break; case PhysicsServer::BODY_MODE_CHARACTER: { _inv_inertia=Vector3(); _inv_mass=1.0/mass; } break; } _update_inertia_tensor(); //_update_shapes(); }
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(); }
void RigidCollisionObjectBullet::remove_shape_full(int p_index) { ERR_FAIL_INDEX(p_index, get_shape_count()); internal_shape_destroy(p_index); shapes.remove(p_index); reload_shapes(); }