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_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,float p_margin_A,float p_margin_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,withMargin> separator(capsule_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;
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();
}
static void _collision_capsule_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) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_A,p_transform_a,convex_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_a.elements[0].normalized()))
return;
//poly vs capsule
for(int i=0;i<convex_B->get_point_count();i++) {
Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
for(int j=0;j<2;j++) {
Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
if (TEST_POINT(capsule_endpoint_A,cpoint ))
return;
}
if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
return;
}
separator.generate_contacts();
}
bool CollisionSolver2DSW::solve_raycast(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,Vector2 *sep_axis) {
const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY)
return false;
Vector2 from = p_transform_A.get_origin();
Vector2 to = from+p_transform_A[1]*ray->get_length();
Vector2 support_A=to;
Matrix32 invb = p_transform_B.affine_inverse();
from = invb.xform(from);
to = invb.xform(to);
Vector2 p,n;
if (!p_shape_B->intersect_segment(from,to,p,n)) {
if (sep_axis)
*sep_axis=p_transform_A[1].normalized();
return false;
}
Vector2 support_B=p_transform_B.xform(p);
if (p_result_callback) {
if (p_swap_result)
p_result_callback(support_B,support_A,p_userdata);
else
p_result_callback(support_A,support_B,p_userdata);
}
return true;
}
void RayCast2D::_notification(int p_what) {
switch(p_what) {
case NOTIFICATION_ENTER_TREE: {
if (enabled && !get_tree()->is_editor_hint())
set_fixed_process(true);
else
set_fixed_process(false);
} break;
case NOTIFICATION_EXIT_TREE: {
if (enabled)
set_fixed_process(false);
} break;
#ifdef TOOLS_ENABLED
case NOTIFICATION_DRAW: {
if (!get_tree()->is_editor_hint())
break;
Matrix32 xf;
xf.rotate(cast_to.atan2());
xf.translate(Vector2(0,cast_to.length()));
//Vector2 tip = Vector2(0,s->get_length());
Color dcol(0.9,0.2,0.2,0.4);
draw_line(Vector2(),cast_to,dcol,3);
Vector<Vector2> pts;
float tsize=4;
pts.push_back(xf.xform(Vector2(0,tsize)));
pts.push_back(xf.xform(Vector2(0.707*tsize,0)));
pts.push_back(xf.xform(Vector2(-0.707*tsize,0)));
Vector<Color> cols;
for(int i=0;i<3;i++)
cols.push_back(dcol);
draw_primitive(pts,cols,Vector<Vector2>()); //small arrow
} break;
#endif
case NOTIFICATION_FIXED_PROCESS: {
if (!enabled)
break;
Ref<World2D> w2d = get_world_2d();
ERR_BREAK( w2d.is_null() );
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(w2d->get_space());
ERR_BREAK( !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)) {
collided=true;
against=rr.collider_id;
collision_point=rr.position;
collision_normal=rr.normal;
against_shape=rr.shape;
} else {
collided=false;
}
} break;
}
}
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();
}
static void _collision_rectangle_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 RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(rectangle_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;
//box faces
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//capsule axis
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
//box endpoints to capsule circles
Matrix32 boxinv = p_transform_a.affine_inverse();
for(int i=0;i<2;i++) {
{
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
return;
}
if (castA) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint-=p_motion_a;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
return;
}
if (castB) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint+=p_motion_b;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
return;
}
if (castA && castB) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint-=p_motion_a;
capsule_endpoint+=p_motion_b;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
return;
}
}
separator.generate_contacts();
}
static void _collision_rectangle_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 RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_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;
//box faces
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//capsule axis
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
//box endpoints to capsule circles
Matrix32 boxinv = p_transform_a.affine_inverse();
for(int i=0;i<2;i++) {
{
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
const Vector2& half_extents = rectangle_A->get_half_extents();
Vector2 local_v = boxinv.xform(capsule_endpoint);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
return;
}
if (castA) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint-=p_motion_a;
const Vector2& half_extents = rectangle_A->get_half_extents();
Vector2 local_v = boxinv.xform(capsule_endpoint);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
return;
}
if (castB) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint+=p_motion_b;
const Vector2& half_extents = rectangle_A->get_half_extents();
Vector2 local_v = boxinv.xform(capsule_endpoint);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
return;
}
if (castA && castB) {
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
capsule_endpoint-=p_motion_a;
capsule_endpoint+=p_motion_b;
const Vector2& half_extents = rectangle_A->get_half_extents();
Vector2 local_v = boxinv.xform(capsule_endpoint);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
return;
}
}
separator.generate_contacts();
}
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) {
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> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
{
Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin());
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
return;
}
if (castA) {
Vector2 sphereofs = sphere + p_motion_a;
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
return;
}
if (castB) {
Vector2 sphereofs = sphere - p_motion_b;
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
return;
}
if (castA && castB) {
Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
Vector2 he(
(local_v.x<0) ? -half_extents.x : half_extents.x,
(local_v.y<0) ? -half_extents.y : half_extents.y
);
if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
return;
}
separator.generate_contacts();
}
