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; }
int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, Physics2DServer::SeparationResult *r_results, int p_result_max, real_t p_margin) { 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)); } // Undo the currently transform the physics server is aware of and apply the provided one body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); Transform2D body_transform = p_transform; for (int i = 0; i < p_result_max; i++) { //reset results r_results[i].collision_depth = 0; } int rays_found = 0; { // raycast AND separate const int max_results = 32; int recover_attempts = 4; Vector2 sr[max_results * 2]; Physics2DServerSW::CollCbkData cbk; cbk.max = max_results; Physics2DServerSW::CollCbkData *cbkptr = &cbk; CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk; do { Vector2 recover_motion; bool collided = false; int amount = _cull_aabb_for_body(p_body, body_aabb); int ray_index = 0; for (int j = 0; j < p_body->get_shape_count(); j++) { if (p_body->is_shape_set_as_disabled(j)) continue; Shape2DSW *body_shape = p_body->get_shape(j); if (body_shape->get_type() != Physics2DServer::SHAPE_RAY) continue; Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; cbk.amount = 0; cbk.ptr = sr; cbk.invalid_by_dir = 0; if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) { continue; } } if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { cbk.valid_dir = body_shape_xform.get_axis(1).normalized(); cbk.valid_depth = p_margin; //only valid depth is the collision margin cbk.invalid_by_dir = 0; } else { cbk.valid_dir = Vector2(); cbk.valid_depth = 0; cbk.invalid_by_dir = 0; } Shape2DSW *against_shape = col_obj->get_shape(shape_idx); if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) { if (cbk.amount > 0) { collided = true; } if (ray_index < p_result_max) { Physics2DServer::SeparationResult &result = r_results[ray_index]; for (int k = 0; k < cbk.amount; k++) { Vector2 a = sr[k * 2 + 0]; Vector2 b = sr[k * 2 + 1]; recover_motion += (b - a) * 0.4; float depth = a.distance_to(b); if (depth > result.collision_depth) { result.collision_depth = depth; result.collision_point = b; result.collision_normal = (b - a).normalized(); result.collision_local_shape = shape_idx; result.collider = col_obj->get_self(); result.collider_id = col_obj->get_instance_id(); result.collider_metadata = col_obj->get_shape_metadata(shape_idx); if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { Body2DSW *body = (Body2DSW *)col_obj; Vector2 rel_vec = b - body->get_transform().get_origin(); result.collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); } } } } } } ray_index++; } rays_found = MAX(ray_index, rays_found); if (!collided || recover_motion == Vector2()) { break; } body_transform.elements[2] += recover_motion; body_aabb.position += recover_motion; recover_attempts--; } while (recover_attempts); } //optimize results (remove non colliding) for (int i = 0; i < rays_found; i++) { if (r_results[i].collision_depth == 0) { rays_found--; SWAP(r_results[i], r_results[rays_found]); } } r_recover_motion = body_transform.elements[2] - p_transform.elements[2]; return rays_found; }
bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t 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)); } // Undo the currently transform the physics server is aware of and apply the provided one body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); Transform2D 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; Physics2DServerSW::CollCbkData *cbkptr = &cbk; CollisionSolver2DSW::CallbackResult 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_disabled(j)) continue; Transform2D 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->is_shape_set_as_one_way_collision(shape_idx)) { cbk.valid_dir = body_shape_xform.get_axis(1).normalized(); cbk.valid_depth = p_margin; //only valid depth is the collision margin } 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; real_t d = rel.length(); if (d==0) continue; Vector2 n = rel/d; real_t traveled = n.dot(recover_motion); a+=n*traveled; real_t d = a.distance_to(b); if (d<margin) continue; #endif recover_motion += (b - a) * 0.4; } if (recover_motion == Vector2()) { collided = false; break; } body_transform.elements[2] += recover_motion; body_aabb.position += recover_motion; recover_attempts--; } while (recover_attempts); } real_t safe = 1.0; real_t unsafe = 1.0; int best_shape = -1; { // STEP 2 ATTEMPT MOTION Rect2 motion_aabb = body_aabb; motion_aabb.position += 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_disabled(j)) continue; Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); bool stuck = false; real_t best_safe = 1; real_t 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]; Transform2D 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->is_shape_set_as_one_way_collision(j)) { continue; } stuck = true; break; } //just do kinematic solving real_t low = 0; real_t hi = 1; Vector2 mnormal = p_motion.normalized(); for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t 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->is_shape_set_as_one_way_collision(j)) { Vector2 cd[2]; Physics2DServerSW::CollCbkData cbk; cbk.max = 1; cbk.amount = 0; cbk.ptr = cd; cbk.valid_dir = body_shape_xform.get_axis(1).normalized(); ; cbk.valid_depth = 10e20; 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.get_origin() - p_from.get_origin()); } } else { //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; ugt.elements[2] += p_motion * unsafe; _RestCallbackData2D rcd; rcd.best_len = 0; rcd.best_object = NULL; rcd.best_shape = 0; Transform2D body_shape_xform = ugt * p_body->get_shape_transform(best_shape); Shape2DSW *body_shape = p_body->get_shape(best_shape); body_aabb.position += 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->is_shape_set_as_one_way_collision(shape_idx)) { rcd.valid_dir = body_shape_xform.get_axis(1).normalized(); rcd.valid_depth = 10e20; } 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_local_shape = 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.get_origin() - p_from.get_origin()); } collided = true; } else { if (r_result) { r_result->motion = p_motion; r_result->remainder = Vector2(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } collided = false; } } return collided; }
bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::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; } AABB 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)); } // Undo the currently transform the physics server is aware of and apply the provided one body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); Transform body_transform = p_from; { //STEP 1, FREE BODY IF STUCK const int max_results = 32; int recover_attempts = 4; Vector3 sr[max_results * 2]; do { PhysicsServerSW::CollCbkData cbk; cbk.max = max_results; cbk.amount = 0; cbk.ptr = sr; PhysicsServerSW::CollCbkData *cbkptr = &cbk; CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_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_disabled(j)) continue; Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); ShapeSW *body_shape = p_body->get_shape(j); for (int i = 0; i < amount; i++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) { collided = cbk.amount > 0; } } } if (!collided) { break; } Vector3 recover_motion; for (int i = 0; i < cbk.amount; i++) { Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; recover_motion += (b - a) * 0.4; } if (recover_motion == Vector3()) { collided = false; break; } body_transform.origin += recover_motion; body_aabb.position += recover_motion; recover_attempts--; } while (recover_attempts); } real_t safe = 1.0; real_t unsafe = 1.0; int best_shape = -1; { // STEP 2 ATTEMPT MOTION AABB motion_aabb = body_aabb; motion_aabb.position += 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_disabled(j)) continue; Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); ShapeSW *body_shape = p_body->get_shape(j); Transform body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShapeSW mshape; mshape.shape = body_shape; mshape.motion = body_shape_xform_inv.basis.xform(p_motion); bool stuck = false; real_t best_safe = 1; real_t best_unsafe = 1; for (int i = 0; i < amount; i++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; Vector3 sep_axis = p_motion.normalized(); Transform 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 (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { //print_line("failed motion cast (no collision)"); continue; } sep_axis = p_motion.normalized(); if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { //print_line("failed motion cast (no collision)"); stuck = true; break; } //just do kinematic solving real_t low = 0; real_t hi = 1; Vector3 mnormal = p_motion.normalized(); for (int i = 0; i < 8; i++) { //steps should be customizable.. real_t ofs = (low + hi) * 0.5; Vector3 sep = mnormal; //important optimization for this to work fast enough mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); Vector3 lA, lB; bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); if (collided) { //print_line(itos(i)+": "+rtos(ofs)); hi = ofs; } else { point_A = lA; point_B = lB; low = ofs; } } 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 = Vector3(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } } else { //it collided, let's get the rest info in unsafe advance Transform ugt = body_transform; ugt.origin += p_motion * unsafe; _RestCallbackData rcd; rcd.best_len = 0; rcd.best_object = NULL; rcd.best_shape = 0; Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape); ShapeSW *body_shape = p_body->get_shape(best_shape); body_aabb.position += p_motion * unsafe; int amount = _cull_aabb_for_body(p_body, body_aabb); for (int i = 0; i < amount; i++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; rcd.object = col_obj; rcd.shape = shape_idx; bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _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_local_shape = 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 BodySW *body = static_cast<const BodySW *>(rcd.best_object); //Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin(); // r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); r_result->motion = safe * p_motion; r_result->remainder = p_motion - safe * p_motion; r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } collided = true; } else { if (r_result) { r_result->motion = p_motion; r_result->remainder = Vector3(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } collided = false; } } return collided; }