void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) {
if (!space)
return;
for(int i=0; i<shapes.size(); i++) {
Shape &s=shapes[i];
if (s.bpid==0) {
s.bpid=space->get_broadphase()->create(this,i);
space->get_broadphase()->set_static(s.bpid,_static);
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb=s.shape->get_aabb();
Matrix32 xform = transform * s.xform;
shape_aabb=xform.xform(shape_aabb);
shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
s.aabb_cache=shape_aabb;
space->get_broadphase()->move(s.bpid,shape_aabb);
}
}
void RayCast2D::_update_raycast_state() {
Ref<World2D> w2d = get_world_2d();
ERR_FAIL_COND( w2d.is_null() );
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(w2d->get_space());
ERR_FAIL_COND( !dss );
Matrix32 gt = get_global_transform();
Vector2 to = cast_to;
if (to==Vector2())
to=Vector2(0,0.01);
Physics2DDirectSpaceState::RayResult rr;
if (dss->intersect_ray(gt.get_origin(),gt.xform(to),rr,exclude,layer_mask,type_mask)) {
collided=true;
against=rr.collider_id;
collision_point=rr.position;
collision_normal=rr.normal;
against_shape=rr.shape;
} else {
collided=false;
}
}
static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
//poly faces and poly points vs circle
for(int i=0;i<convex_B->get_point_count();i++) {
if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) ))
return;
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
return;
}
separator.generate_contacts();
}
void TileMap::_update_quadrant_transform() {
if (!is_inside_tree())
return;
Matrix32 global_transform = get_global_transform();
Matrix32 nav_rel;
if (navigation)
nav_rel = get_relative_transform_to_parent(navigation);
for (Map<PosKey,Quadrant>::Element *E=quadrant_map.front();E;E=E->next()) {
Quadrant &q=E->get();
Matrix32 xform;
xform.set_origin( q.pos );
xform = global_transform * xform;
Physics2DServer::get_singleton()->body_set_state(q.body,Physics2DServer::BODY_STATE_TRANSFORM,xform);
if (navigation) {
for(Map<PosKey,Quadrant::NavPoly>::Element *E=q.navpoly_ids.front();E;E=E->next()) {
navigation->navpoly_set_transform(E->get().id,nav_rel * E->get().xform);
}
}
for(Map<PosKey,Quadrant::Occluder>::Element *E=q.occluder_instances.front();E;E=E->next()) {
VS::get_singleton()->canvas_light_occluder_set_transform(E->get().id,global_transform * E->get().xform);
}
}
}
void Node2D::edit_set_rect(const Rect2& p_edit_rect) {
Rect2 r = get_item_rect();
Vector2 zero_offset;
if (r.size.x!=0)
zero_offset.x = -r.pos.x / r.size.x;
if (r.size.y!=0)
zero_offset.y = -r.pos.y / r.size.y;
Size2 new_scale(1,1);
if (r.size.x!=0)
new_scale.x = p_edit_rect.size.x / r.size.x;
if (r.size.y!=0)
new_scale.y = p_edit_rect.size.y / r.size.y;
Point2 new_pos = p_edit_rect.pos + p_edit_rect.size*zero_offset;//p_edit_rect.pos - r.pos;
Matrix32 postxf;
postxf.set_rotation_and_scale(angle,_scale);
new_pos = postxf.xform(new_pos);
pos+=new_pos;
_scale*=new_scale;
_update_transform();
_change_notify("transform/scale");
_change_notify("transform/pos");
}
RID DampedSpringJoint2D::_configure_joint(){
Node *node_a = has_node( get_node_a() ) ? get_node( get_node_a() ) : (Node*)NULL;
Node *node_b = has_node( get_node_b() ) ? get_node( get_node_b() ) : (Node*)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a=node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b=node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(),body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(),body_b->get_rid());
Matrix32 gt = get_global_transform();
Vector2 anchor_A = gt.get_origin();
Vector2 anchor_B = gt.xform( Vector2(0,length) );
RID dsj = Physics2DServer::get_singleton()->damped_spring_joint_create(anchor_A,anchor_B,body_a->get_rid(),body_b->get_rid());
if (rest_length)
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj,Physics2DServer::DAMPED_STRING_REST_LENGTH,rest_length);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj,Physics2DServer::DAMPED_STRING_STIFFNESS,stiffness);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj,Physics2DServer::DAMPED_STRING_DAMPING,damping);
return dsj;
}
void CollisionObject2DSW::_update_shapes() {
if (!space)
return;
for(int i=0; i<shapes.size(); i++) {
Shape &s=shapes[i];
if (s.bpid==0) {
s.bpid=space->get_broadphase()->create(this,i);
space->get_broadphase()->set_static(s.bpid,_static);
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb=s.shape->get_aabb();
Matrix32 xform = transform * s.xform;
shape_aabb=xform.xform(shape_aabb);
s.aabb_cache=shape_aabb;
s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
space->get_broadphase()->move(s.bpid,s.aabb_cache);
}
}
static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
//capsule axis
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
//capsule endpoints
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
separator.generate_contacts();
}
Map<TileMap::PosKey,TileMap::Quadrant>::Element *TileMap::_create_quadrant(const PosKey& p_qk) {
Matrix32 xform;
//xform.set_origin(Point2(p_qk.x,p_qk.y)*cell_size*quadrant_size);
Quadrant q;
q.pos = _map_to_world(p_qk.x*_get_quadrant_size(),p_qk.y*_get_quadrant_size());
q.pos+=get_cell_draw_offset();
if (tile_origin==TILE_ORIGIN_CENTER)
q.pos+=cell_size/2;
else if (tile_origin==TILE_ORIGIN_BOTTOM_LEFT)
q.pos.y+=cell_size.y;
xform.set_origin( q.pos );
// q.canvas_item = VisualServer::get_singleton()->canvas_item_create();
q.body=Physics2DServer::get_singleton()->body_create(use_kinematic?Physics2DServer::BODY_MODE_KINEMATIC:Physics2DServer::BODY_MODE_STATIC);
Physics2DServer::get_singleton()->body_attach_object_instance_ID(q.body,get_instance_ID());
Physics2DServer::get_singleton()->body_set_layer_mask(q.body,collision_layer);
Physics2DServer::get_singleton()->body_set_collision_mask(q.body,collision_mask);
Physics2DServer::get_singleton()->body_set_param(q.body,Physics2DServer::BODY_PARAM_FRICTION,friction);
Physics2DServer::get_singleton()->body_set_param(q.body,Physics2DServer::BODY_PARAM_BOUNCE,bounce);
if (is_inside_tree()) {
xform = get_global_transform() * xform;
RID space = get_world_2d()->get_space();
Physics2DServer::get_singleton()->body_set_space(q.body,space);
}
Physics2DServer::get_singleton()->body_set_state(q.body,Physics2DServer::BODY_STATE_TRANSFORM,xform);
rect_cache_dirty=true;
quadrant_order_dirty=true;
return quadrant_map.insert(p_qk,q);
}
RID GrooveJoint2D::_configure_joint(){
Node *node_a = has_node( get_node_a() ) ? get_node( get_node_a() ) : (Node*)NULL;
Node *node_b = has_node( get_node_b() ) ? get_node( get_node_b() ) : (Node*)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a=node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b=node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(),body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(),body_b->get_rid());
Matrix32 gt = get_global_transform();
Vector2 groove_A1 = gt.get_origin();
Vector2 groove_A2 = gt.xform( Vector2(0,length) );
Vector2 anchor_B = gt.xform( Vector2(0,initial_offset) );
return Physics2DServer::get_singleton()->groove_joint_create(groove_A1,groove_A2,anchor_B,body_a->get_rid(),body_b->get_rid());
}
InputEvent InputEvent::xform_by(const Matrix32& p_xform) const {
InputEvent ev=*this;
switch(ev.type) {
case InputEvent::MOUSE_BUTTON: {
Vector2 g = p_xform.xform(Vector2(ev.mouse_button.global_x,ev.mouse_button.global_y));
Vector2 l = p_xform.xform(Vector2(ev.mouse_button.x,ev.mouse_button.y));
ev.mouse_button.x=l.x;
ev.mouse_button.y=l.y;
ev.mouse_button.global_x=g.x;
ev.mouse_button.global_y=g.y;
} break;
case InputEvent::MOUSE_MOTION: {
Vector2 g = p_xform.xform(Vector2(ev.mouse_motion.global_x,ev.mouse_motion.global_y));
Vector2 l = p_xform.xform(Vector2(ev.mouse_motion.x,ev.mouse_motion.y));
Vector2 r = p_xform.basis_xform(Vector2(ev.mouse_motion.relative_x,ev.mouse_motion.relative_y));
Vector2 s = p_xform.basis_xform(Vector2(ev.mouse_motion.speed_x,ev.mouse_motion.speed_y));
ev.mouse_motion.x=l.x;
ev.mouse_motion.y=l.y;
ev.mouse_motion.global_x=g.x;
ev.mouse_motion.global_y=g.y;
ev.mouse_motion.relative_x=r.x;
ev.mouse_motion.relative_y=r.y;
ev.mouse_motion.speed_x=s.x;
ev.mouse_motion.speed_y=s.y;
} break;
case InputEvent::SCREEN_TOUCH: {
Vector2 t = p_xform.xform(Vector2(ev.screen_touch.x,ev.screen_touch.y));
ev.screen_touch.x=t.x;
ev.screen_touch.y=t.y;
} break;
case InputEvent::SCREEN_DRAG: {
Vector2 t = p_xform.xform(Vector2(ev.screen_drag.x,ev.screen_drag.y));
Vector2 r = p_xform.basis_xform(Vector2(ev.screen_drag.relative_x,ev.screen_drag.relative_y));
Vector2 s = p_xform.basis_xform(Vector2(ev.screen_drag.speed_x,ev.screen_drag.speed_y));
ev.screen_drag.x=t.x;
ev.screen_drag.y=t.y;
ev.screen_drag.relative_x=r.x;
ev.screen_drag.relative_y=r.y;
ev.screen_drag.speed_x=s.x;
ev.screen_drag.speed_y=s.y;
} break;
}
return ev;
}
static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
//box faces
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//convex faces
Matrix32 boxinv;
if (withMargin) {
boxinv=p_transform_a.affine_inverse();
}
for(int i=0;i<convex_B->get_point_count();i++) {
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
return;
if (withMargin) {
//all points vs all points need to be tested if margin exist
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i)))))
return;
if (castA) {
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a)))
return;
}
if (castB) {
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b)))
return;
}
if (castA && castB) {
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a)))
return;
}
}
}
separator.generate_contacts();
}
void Node2D::set_global_pos(const Point2& p_pos) {
Matrix32 inv;
CanvasItem *pi = get_parent_item();
if (pi) {
inv = pi->get_global_transform().affine_inverse();
set_pos(inv.xform(p_pos));
} else {
set_pos(p_pos);
}
}
bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) {
Vector2 motion = p_A->get_linear_velocity()*p_step;
real_t mlen = motion.length();
if (mlen<CMP_EPSILON)
return false;
Vector2 mnormal = motion / mlen;
real_t min,max;
p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
return false;
}
//cast a segment from support in motion normal, in the same direction of motion by motion length
//support is the worst case collision point, so real collision happened before
int a;
Vector2 s[2];
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
Vector2 from = p_xform_A.xform(s[0]);
Vector2 to = from + motion;
Matrix32 from_inv = p_xform_B.affine_inverse();
Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
Vector2 local_to = from_inv.xform(to);
Vector2 rpos,rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
return false;
//ray hit something
Vector2 hitpos = p_xform_B.xform(rpos);
Vector2 contact_A = to;
Vector2 contact_B = hitpos;
//create a contact
if (p_swap_result)
_contact_added_callback(contact_B,contact_A);
else
_contact_added_callback(contact_A,contact_B);
return true;
}
static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
return;
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
separator.generate_contacts();
}
void Viewport::set_canvas_transform(const Matrix32& p_transform) {
canvas_transform=p_transform;
VisualServer::get_singleton()->viewport_set_canvas_transform(viewport,find_world_2d()->get_canvas(),canvas_transform);
Matrix32 xform = (global_canvas_transform * canvas_transform).affine_inverse();
Size2 ss = get_visible_rect().size;
SpatialSound2DServer::get_singleton()->listener_set_transform(listener_2d,Matrix32(0,xform.xform(ss*0.5)));
Vector2 ss2 = ss*xform.get_scale();
float panrange = MAX(ss2.x,ss2.y);
SpatialSound2DServer::get_singleton()->listener_set_param(listener_2d,SpatialSound2DServer::LISTENER_PARAM_PAN_RANGE,panrange);
}
void TileMap::_update_quadrant_transform() {
if (!is_inside_scene())
return;
Matrix32 global_transform = get_global_transform();
for (Map<PosKey,Quadrant>::Element *E=quadrant_map.front();E;E=E->next()) {
Quadrant &q=E->get();
Matrix32 xform;
xform.set_origin( q.pos );
xform = global_transform * xform;
Physics2DServer::get_singleton()->body_set_state(q.static_body,Physics2DServer::BODY_STATE_TRANSFORM,xform);
}
}
void Viewport::_update_global_transform() {
Matrix32 sxform = stretch_transform * global_canvas_transform;
VisualServer::get_singleton()->viewport_set_global_canvas_transform(viewport,sxform);
Matrix32 xform = (sxform * canvas_transform).affine_inverse();
Size2 ss = get_visible_rect().size;
SpatialSound2DServer::get_singleton()->listener_set_transform(listener_2d,Matrix32(0,xform.xform(ss*0.5)));
Vector2 ss2 = ss*xform.get_scale();
float panrange = MAX(ss2.x,ss2.y);
SpatialSound2DServer::get_singleton()->listener_set_param(listener_2d,SpatialSound2DServer::LISTENER_PARAM_PAN_RANGE,panrange);
}
void Area2DSW::set_transform(const Matrix32& p_transform) {
if (!moved_list.in_list() && get_space())
get_space()->area_add_to_moved_list(&moved_list);
_set_transform(p_transform);
_set_inv_transform(p_transform.affine_inverse());
}
void NavigationPolygonEditor::_canvas_draw() {
if (!node)
return;
Control *vpc = canvas_item_editor->get_viewport_control();
if (node->get_navigation_polygon().is_null())
return;
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle= get_icon("EditorHandle","EditorIcons");
for(int j=-1;j<node->get_navigation_polygon()->get_outline_count();j++) {
Vector<Vector2> poly;
if (wip_active && j==edited_outline) {
poly=wip;
} else {
if (j==-1)
continue;
poly = Variant(node->get_navigation_polygon()->get_outline(j));
}
int len = poly.size();
for(int i=0;i<poly.size();i++) {
Vector2 p,p2;
p = (j==edited_outline && i==edited_point) ? edited_point_pos : poly[i];
if (j==edited_outline && ((wip_active && i==poly.size()-1) || (((i+1)%poly.size())==edited_point)))
p2=edited_point_pos;
else
p2 = poly[(i+1)%poly.size()];
Vector2 point = xform.xform(p);
Vector2 next_point = xform.xform(p2);
Color col=Color(1,0.3,0.1,0.8);
vpc->draw_line(point,next_point,col,2);
vpc->draw_texture(handle,point-handle->get_size()*0.5);
}
}
}
bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape,0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
aabb=aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
_RestCallbackData rcd;
rcd.best_len=0;
rcd.best_object=NULL;
rcd.best_shape=0;
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;
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
if (p_exclude.has( col_obj->get_self() ))
continue;
rcd.object=col_obj;
rcd.shape=shape_idx;
bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,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)
return false;
r_info->collider_id=rcd.best_object->get_instance_id();
r_info->shape=rcd.best_shape;
r_info->normal=rcd.best_normal;
r_info->point=rcd.best_contact;
r_info->rid=rcd.best_object->get_self();
if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
Vector2 rel_vec = r_info->point-body->get_transform().get_origin();
r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
} else {
r_info->linear_velocity=Vector2();
}
return true;
}
void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_transform) {
ERR_FAIL_INDEX(p_index,shapes.size());
shapes[p_index].xform=p_transform;
shapes[p_index].xform_inv=p_transform.affine_inverse();
_update_shapes();
_shapes_changed();
}
void LightOccluder2DEditor::_canvas_draw() {
if (!node || !node->get_occluder_polygon().is_valid())
return;
Control *vpc = canvas_item_editor->get_viewport_control();
Vector<Vector2> poly;
if (wip_active)
poly=wip;
else
poly=Variant(node->get_occluder_polygon()->get_polygon());
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle= get_icon("EditorHandle","EditorIcons");
int len = poly.size();
for(int i=0;i<poly.size();i++) {
Vector2 p,p2;
p = i==edited_point ? edited_point_pos : poly[i];
if ((wip_active && i==poly.size()-1) || (((i+1)%poly.size())==edited_point))
p2=edited_point_pos;
else
p2 = poly[(i+1)%poly.size()];
Vector2 point = xform.xform(p);
Vector2 next_point = xform.xform(p2);
Color col=Color(1,0.3,0.1,0.8);
if (i==poly.size()-1 && (!node->get_occluder_polygon()->is_closed() || wip_active)) {
} else {
vpc->draw_line(point,next_point,col,2);
}
vpc->draw_texture(handle,point-handle->get_size()*0.5);
}
}
bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE)
return false;
Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized();
Vector2 p = p_transform_A.xform(line->get_normal()*line->get_d());
real_t d = n.dot(p);
Vector2 supports[2];
int support_count;
p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count);
bool found=false;
for(int i=0;i<support_count;i++) {
supports[i] = p_transform_B.xform( supports[i] );
real_t pd = n.dot(supports[i]);
if (pd>=d)
continue;
found=true;
Vector2 support_A = supports[i] - n*(pd-d);
if (p_result_callback) {
if (p_swap_result)
p_result_callback(supports[i],support_A,p_userdata);
else
p_result_callback(support_A,supports[i],p_userdata);
}
}
return found;
}
void Path2DEditor::_canvas_draw() {
if (!node)
return ;
if (!node->is_visible())
return;
if (!node->get_curve().is_valid())
return ;
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle= get_icon("EditorHandle","EditorIcons");
Size2 handle_size = handle->get_size();
Ref<Curve2D> curve = node->get_curve();
int len = curve->get_point_count();
Control *vpc = canvas_item_editor->get_viewport_control();
for(int i=0;i<len;i++) {
Vector2 point = xform.xform(curve->get_point_pos(i));
vpc->draw_texture_rect(handle,Rect2(point-handle_size*0.5,handle_size),false,Color(1,1,1,1));
if (i<len-1) {
Vector2 pointout = xform.xform(curve->get_point_pos(i)+curve->get_point_out(i));
vpc->draw_line(point,pointout,Color(0.5,0.5,1.0,0.8),1.0);
vpc->draw_texture_rect(handle, Rect2(pointout-handle_size*0.5,handle_size),false,Color(1,0.5,1,0.3));
}
if (i>0) {
Vector2 pointin = xform.xform(curve->get_point_pos(i)+curve->get_point_in(i));
vpc->draw_line(point,pointin,Color(0.5,0.5,1.0,0.8),1.0);
vpc->draw_texture_rect(handle, Rect2(pointin-handle_size*0.5,handle_size),false,Color(1,0.5,1,0.3));
}
}
}
static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
const Vector2 &sphere=p_transform_a.elements[2];
const Vector2 *axis=&p_transform_b.elements[0];
// const Vector2& half_extents = rectangle_B->get_half_extents();
if (!separator.test_axis(axis[0].normalized()))
return;
if (!separator.test_axis(axis[1].normalized()))
return;
Matrix32 binv = p_transform_b.affine_inverse();
{
if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) )
return;
}
if (castA) {
Vector2 sphereofs = sphere + p_motion_a;
if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
return;
}
if (castB) {
Vector2 sphereofs = sphere - p_motion_b;
if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
return;
}
if (castA && castB) {
Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
return;
}
separator.generate_contacts();
}
bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
if (p_result_max<=0)
return 0;
Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape,0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
aabb=aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
bool collided=false;
int cc=0;
r_result_count=0;
Physics2DServerSW::CollCbkData cbk;
cbk.max=p_result_max;
cbk.amount=0;
cbk.ptr=r_results;
CollisionSolver2DSW::CallbackResult cbkres=NULL;
Physics2DServerSW::CollCbkData *cbkptr=NULL;
if (p_result_max>0) {
cbkptr=&cbk;
cbkres=Physics2DServerSW::_shape_col_cbk;
}
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;
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
if (p_exclude.has( col_obj->get_self() ))
continue;
if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
collided=true;
}
}
r_result_count=cbk.amount;
return collided;
}
static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
//capsule axis
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
return;
//capsule endpoints
for(int i=0;i<2;i++) {
Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5);
for(int j=0;j<2;j++) {
Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
return;
}
}
separator.generate_contacts();
}
static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
if (!separator.test_previous_axis())
return;
if (!separator.test_cast())
return;
if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin()))
return;
separator.generate_contacts();
}
Map<TileMap::PosKey,TileMap::Quadrant>::Element *TileMap::_create_quadrant(const PosKey& p_qk) {
Matrix32 xform;
xform.set_origin(Point2(p_qk.x,p_qk.y)*quadrant_size*cell_size);
Quadrant q;
q.canvas_item = VisualServer::get_singleton()->canvas_item_create();
VisualServer::get_singleton()->canvas_item_set_parent( q.canvas_item, get_canvas_item() );
VisualServer::get_singleton()->canvas_item_set_transform( q.canvas_item, xform );
q.static_body=Physics2DServer::get_singleton()->body_create(Physics2DServer::BODY_MODE_STATIC);
if (is_inside_scene()) {
xform = get_global_transform() * xform;
RID space = get_world_2d()->get_space();
Physics2DServer::get_singleton()->body_set_space(q.static_body,space);
}
Physics2DServer::get_singleton()->body_set_state(q.static_body,Physics2DServer::BODY_STATE_TRANSFORM,xform);
q.pos=Vector2(p_qk.x,p_qk.y)*quadrant_size*cell_size;
rect_cache_dirty=true;
return quadrant_map.insert(p_qk,q);
}