bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) { ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ERR_FAIL_COND_V(!shape,false); Rect3 aabb = p_xform.xform(shape->get_aabb()); aabb=aabb.merge(Rect3(aabb.pos+p_motion,aabb.size)); //motion aabb=aabb.grow(p_margin); /* if (p_motion!=Vector3()) print_line(p_motion); */ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results); float best_safe=1; float best_unsafe=1; Transform xform_inv = p_xform.affine_inverse(); MotionShapeSW mshape; mshape.shape=shape; mshape.motion=xform_inv.basis.xform(p_motion); bool best_first=true; Vector3 closest_A,closest_B; for(int i=0;i<amount;i++) { if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask)) continue; if (p_exclude.has( space->intersection_query_results[i]->get_self())) continue; //ignore excluded const CollisionObjectSW *col_obj=space->intersection_query_results[i]; int shape_idx=space->intersection_query_subindex_results[i]; 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,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) { //print_line("failed motion cast (no collision)"); continue; } //test initial overlap #if 0 if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) { print_line("failed initial cast (collision at begining)"); return false; } #else sep_axis=p_motion.normalized(); if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) { //print_line("failed motion cast (no collision)"); return false; } #endif //just do kinematic solving float low=0; float hi=1; Vector3 mnormal=p_motion.normalized(); for(int i=0;i<8;i++) { //steps should be customizable.. float ofs = (low+hi)*0.5; Vector3 sep=mnormal; //important optimization for this to work fast enough mshape.motion=xform_inv.basis.xform(p_motion*ofs); Vector3 lA,lB; bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,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_first=true; //force reset best_safe=low; best_unsafe=hi; } if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) { closest_A=point_A; closest_B=point_B; r_info->collider_id=col_obj->get_instance_id(); r_info->rid=col_obj->get_self(); r_info->shape=shape_idx; r_info->point=closest_B; r_info->normal=(closest_A-closest_B).normalized(); best_first=false; if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) { const BodySW *body=static_cast<const BodySW*>(col_obj); r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B); } } } p_closest_safe=best_safe; p_closest_unsafe=best_unsafe; return true; }