You've already forked godot
mirror of
https://github.com/godotengine/godot.git
synced 2025-11-04 12:00:25 +00:00
-added kinematic body
-added kinematic body demos
This commit is contained in:
Juan Linietsky
@@ -62,6 +62,7 @@ class Body2DSW : public CollisionObject2DSW {
|
||||
Vector2 applied_force;
|
||||
real_t applied_torque;
|
||||
|
||||
|
||||
SelfList<Body2DSW> active_list;
|
||||
SelfList<Body2DSW> inertia_update_list;
|
||||
SelfList<Body2DSW> direct_state_query_list;
|
||||
|
||||
@@ -434,8 +434,9 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const
|
||||
if (E->key()->pass==pass)
|
||||
continue;
|
||||
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb))
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
|
||||
continue;
|
||||
|
||||
@@ -218,4 +218,5 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
|
||||
type=p_type;
|
||||
space=NULL;
|
||||
instance_id=0;
|
||||
user_mask=0;
|
||||
}
|
||||
|
||||
@@ -65,6 +65,7 @@ private:
|
||||
Space2DSW *space;
|
||||
Matrix32 transform;
|
||||
Matrix32 inv_transform;
|
||||
uint32_t user_mask;
|
||||
bool _static;
|
||||
|
||||
void _update_shapes();
|
||||
@@ -117,6 +118,8 @@ public:
|
||||
_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
|
||||
|
||||
|
||||
void set_user_mask(uint32_t p_mask) {user_mask=p_mask;}
|
||||
_FORCE_INLINE_ uint32_t get_user_mask() const { return user_mask; }
|
||||
|
||||
void remove_shape(Shape2DSW *p_shape);
|
||||
void remove_shape(int p_index);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -32,6 +32,6 @@
|
||||
#include "collision_solver_2d_sw.h"
|
||||
|
||||
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
|
||||
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
|
||||
|
||||
#endif // COLLISION_SOLVER_2D_SAT_H
|
||||
|
||||
@@ -150,6 +150,8 @@ struct _ConcaveCollisionInfo2D {
|
||||
const Matrix32 *transform_B;
|
||||
Vector2 motion_A;
|
||||
Vector2 motion_B;
|
||||
real_t margin_A;
|
||||
real_t margin_B;
|
||||
CollisionSolver2DSW::CallbackResult result_callback;
|
||||
void *userdata;
|
||||
bool swap_result;
|
||||
@@ -169,7 +171,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
|
||||
if (!cinfo.result_callback && cinfo.collided)
|
||||
return; //already collided and no contacts requested, don't test anymore
|
||||
|
||||
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
|
||||
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B );
|
||||
if (!collided)
|
||||
return;
|
||||
|
||||
@@ -179,7 +181,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
|
||||
|
||||
}
|
||||
|
||||
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
|
||||
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
|
||||
|
||||
|
||||
const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
|
||||
@@ -195,6 +197,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
|
||||
cinfo.collided=false;
|
||||
cinfo.collisions=0;
|
||||
cinfo.sep_axis=sep_axis;
|
||||
cinfo.margin_A=p_margin_A;
|
||||
cinfo.margin_B=p_margin_B;
|
||||
|
||||
cinfo.aabb_tests=0;
|
||||
|
||||
@@ -227,7 +231,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
|
||||
}
|
||||
|
||||
|
||||
bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
|
||||
bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
|
||||
|
||||
|
||||
|
||||
@@ -236,12 +240,14 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
|
||||
Physics2DServer::ShapeType type_B=p_shape_B->get_type();
|
||||
bool concave_A=p_shape_A->is_concave();
|
||||
bool concave_B=p_shape_B->is_concave();
|
||||
real_t margin_A=p_margin_A,margin_B=p_margin_B;
|
||||
|
||||
bool swap = false;
|
||||
|
||||
if (type_A>type_B) {
|
||||
SWAP(type_A,type_B);
|
||||
SWAP(concave_A,concave_B);
|
||||
SWAP(margin_A,margin_B);
|
||||
swap=true;
|
||||
}
|
||||
|
||||
@@ -292,16 +298,16 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
|
||||
return false;
|
||||
|
||||
if (!swap)
|
||||
return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
|
||||
else
|
||||
return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis);
|
||||
return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B);
|
||||
|
||||
|
||||
|
||||
} else {
|
||||
|
||||
|
||||
return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis);
|
||||
return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -37,14 +37,14 @@ public:
|
||||
private:
|
||||
static bool 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);
|
||||
static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
|
||||
static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
|
||||
static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
|
||||
static bool 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=NULL);
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
|
||||
static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -132,8 +132,12 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
|
||||
|
||||
void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
|
||||
|
||||
|
||||
CollCbkData *cbk=(CollCbkData *)p_userdata;
|
||||
|
||||
if (cbk->max==0)
|
||||
return;
|
||||
|
||||
if (cbk->amount == cbk->max) {
|
||||
//find least deep
|
||||
float min_depth=1e20;
|
||||
@@ -159,6 +163,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p
|
||||
|
||||
cbk->ptr[cbk->amount*2+0]=p_point_A;
|
||||
cbk->ptr[cbk->amount*2+1]=p_point_B;
|
||||
cbk->amount++;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -648,19 +653,20 @@ uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const {
|
||||
};
|
||||
|
||||
|
||||
void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
|
||||
void Physics2DServerSW::body_set_user_mask(RID p_body, uint32_t p_flags) {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND(!body);
|
||||
body->set_user_mask(p_flags);
|
||||
|
||||
};
|
||||
|
||||
uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
|
||||
uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const {
|
||||
|
||||
Body2DSW *body = body_owner.get(p_body);
|
||||
ERR_FAIL_COND_V(!body,0);
|
||||
|
||||
return 0;
|
||||
return body->get_user_mask();
|
||||
};
|
||||
|
||||
void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
|
||||
|
||||
@@ -58,6 +58,12 @@ friend class Physics2DDirectSpaceStateSW;
|
||||
mutable RID_Owner<Body2DSW> body_owner;
|
||||
mutable RID_Owner<Joint2DSW> joint_owner;
|
||||
|
||||
|
||||
|
||||
|
||||
// void _clear_query(Query2DSW *p_query);
|
||||
public:
|
||||
|
||||
struct CollCbkData {
|
||||
|
||||
int max;
|
||||
@@ -68,9 +74,6 @@ friend class Physics2DDirectSpaceStateSW;
|
||||
static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
|
||||
|
||||
|
||||
// void _clear_query(Query2DSW *p_query);
|
||||
public:
|
||||
|
||||
virtual RID shape_create(ShapeType p_shape);
|
||||
virtual void shape_set_data(RID p_shape, const Variant& p_data);
|
||||
virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
|
||||
@@ -158,8 +161,8 @@ public:
|
||||
virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
|
||||
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
|
||||
|
||||
virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
|
||||
virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
|
||||
virtual void body_set_user_mask(RID p_body, uint32_t p_mask);
|
||||
virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const;
|
||||
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
|
||||
virtual float body_get_param(RID p_body, BodyParameter p_param) const;
|
||||
|
||||
@@ -354,6 +354,51 @@ public:
|
||||
r_max = distance + length;
|
||||
}
|
||||
|
||||
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const Vector2& p_circle) const {
|
||||
|
||||
Vector2 local_v = p_xform_inv.xform(p_circle);
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
return (p_xform.xform(he)-p_circle).normalized();
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ Vector2 get_box_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const RectangleShape2DSW *p_B,const Matrix32& p_B_xform, const Matrix32& p_B_xform_inv) const {
|
||||
|
||||
Vector2 a,b;
|
||||
|
||||
{
|
||||
Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -half_extents.x : half_extents.x,
|
||||
(local_v.y<0) ? -half_extents.y : half_extents.y
|
||||
);
|
||||
|
||||
a=p_xform.xform(he);
|
||||
|
||||
}
|
||||
{
|
||||
Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
|
||||
|
||||
Vector2 he(
|
||||
(local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x,
|
||||
(local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y
|
||||
);
|
||||
|
||||
b=p_B_xform.xform(he);
|
||||
|
||||
}
|
||||
|
||||
return (a-b).normalized();
|
||||
}
|
||||
|
||||
|
||||
DEFAULT_PROJECT_RANGE_CAST
|
||||
|
||||
};
|
||||
|
||||
@@ -31,7 +31,22 @@
|
||||
#include "physics_2d_server_sw.h"
|
||||
|
||||
|
||||
bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
_FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_user_mask, uint32_t p_type_mask) {
|
||||
|
||||
if (p_user_mask && !(p_object->get_user_mask()&p_user_mask))
|
||||
return false;
|
||||
|
||||
if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA && !(p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA))
|
||||
return false;
|
||||
|
||||
Body2DSW *body = static_cast<Body2DSW*>(p_object);
|
||||
|
||||
return (1<<body->get_mode())&p_type_mask;
|
||||
|
||||
}
|
||||
|
||||
bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
|
||||
|
||||
|
||||
|
||||
ERR_FAIL_COND_V(space->locked,false);
|
||||
@@ -55,8 +70,8 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
|
||||
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
|
||||
continue; //ignore area
|
||||
if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
|
||||
continue;
|
||||
@@ -120,7 +135,7 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
|
||||
}
|
||||
|
||||
|
||||
int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
|
||||
|
||||
if (p_result_max<=0)
|
||||
return 0;
|
||||
@@ -129,6 +144,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
|
||||
ERR_FAIL_COND_V(!shape,0);
|
||||
|
||||
Rect2 aabb = p_xform.xform(shape->get_aabb());
|
||||
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);
|
||||
|
||||
@@ -137,11 +153,8 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
|
||||
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
if (cc>=p_result_max)
|
||||
break;
|
||||
|
||||
if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
|
||||
continue; //ignore area
|
||||
if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
|
||||
continue;
|
||||
@@ -150,7 +163,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
|
||||
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
|
||||
int shape_idx=space->intersection_query_subindex_results[i];
|
||||
|
||||
if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL))
|
||||
if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin))
|
||||
continue;
|
||||
|
||||
r_results[cc].collider_id=col_obj->get_instance_id();
|
||||
@@ -168,79 +181,31 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
|
||||
}
|
||||
|
||||
|
||||
struct MotionCallbackRayCastData {
|
||||
|
||||
Vector2 best_contact;
|
||||
Vector2 best_normal;
|
||||
float best_len;
|
||||
Matrix32 b_xform_inv;
|
||||
Matrix32 b_xform;
|
||||
Vector2 motion;
|
||||
Shape2DSW * shape_B;
|
||||
|
||||
};
|
||||
|
||||
static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
|
||||
bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
|
||||
|
||||
|
||||
MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata;
|
||||
|
||||
Vector2 contact_normal = (p_point_B-p_point_A).normalized();
|
||||
|
||||
Vector2 from=p_point_A-(rd->motion*1.01);
|
||||
Vector2 p,n;
|
||||
|
||||
if (contact_normal.dot(rd->motion.normalized())<CMP_EPSILON) {
|
||||
//safe to assume it was a perpendicular collision
|
||||
n=contact_normal;
|
||||
p=p_point_B;
|
||||
} else {
|
||||
//entered in a different angle
|
||||
Vector2 to = p_point_A+rd->motion; //avoid precission issues
|
||||
|
||||
|
||||
bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n);
|
||||
|
||||
|
||||
if (!res) {
|
||||
print_line("lolwut failed");
|
||||
return;
|
||||
}
|
||||
|
||||
p = rd->b_xform.xform(p);
|
||||
|
||||
n = rd->b_xform_inv.basis_xform_inv(n).normalized();
|
||||
}
|
||||
|
||||
float len = p.distance_to(from);
|
||||
|
||||
if (len<rd->best_len) {
|
||||
rd->best_contact=p;
|
||||
rd->best_normal=n;
|
||||
rd->best_len=len;
|
||||
}
|
||||
}
|
||||
|
||||
bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
|
||||
Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
|
||||
ERR_FAIL_COND_V(!shape,0);
|
||||
ERR_FAIL_COND_V(!shape,false);
|
||||
|
||||
Rect2 aabb = p_xform.xform(shape->get_aabb());
|
||||
aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
|
||||
aabb=aabb.grow(p_margin);
|
||||
|
||||
//if (p_motion!=Vector2())
|
||||
// print_line(p_motion);
|
||||
|
||||
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
|
||||
|
||||
bool collided=false;
|
||||
r_result.travel=1;
|
||||
float best_safe=1;
|
||||
float best_unsafe=1;
|
||||
|
||||
MotionCallbackRayCastData best_normal;
|
||||
best_normal.best_len=1e20;
|
||||
for(int i=0;i<amount;i++) {
|
||||
|
||||
|
||||
if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
|
||||
continue; //ignore area
|
||||
if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
|
||||
continue; //ignore excluded
|
||||
@@ -252,54 +217,17 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32
|
||||
|
||||
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(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
|
||||
|
||||
if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
//test initial overlap
|
||||
if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
|
||||
if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
|
||||
|
||||
r_result.collider_id=col_obj->get_instance_id();
|
||||
r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
|
||||
r_result.shape=shape_idx;
|
||||
r_result.rid=col_obj->get_self();
|
||||
r_result.travel=0;
|
||||
r_result.point=Vector2();
|
||||
r_result.normal=Vector2();
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
#if 0
|
||||
Vector2 mnormal=p_motion.normalized();
|
||||
Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
||||
ShapeSW *col_shape = col_obj->get_shape(shape_idx);
|
||||
|
||||
real_t min,max;
|
||||
col_shape->project_rangev(mnormal,col_shape_xform,min,max);
|
||||
real_t width = max-min;
|
||||
|
||||
int a;
|
||||
Vector2 s[2];
|
||||
col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a);
|
||||
Vector2 from = col_shape_xform.xform(s[0]);
|
||||
Vector2 to = from + p_motion;
|
||||
|
||||
Matrix32 from_inv = col_shape_xform.affine_inverse();
|
||||
|
||||
Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box
|
||||
Vector2 local_to = from_inv.xform(to);
|
||||
|
||||
Vector2 rpos,rnorm;
|
||||
if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm))
|
||||
return false;
|
||||
|
||||
//ray hit something
|
||||
|
||||
|
||||
Vector2 hitpos = p_xform_B.xform(rpos);
|
||||
#endif
|
||||
|
||||
//just do kinematic solving
|
||||
float low=0;
|
||||
@@ -312,7 +240,7 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32
|
||||
float ofs = (low+hi)*0.5;
|
||||
|
||||
Vector2 sep=mnormal; //important optimization for this to work fast enough
|
||||
bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep);
|
||||
bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin);
|
||||
|
||||
if (collided) {
|
||||
|
||||
@@ -323,38 +251,165 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32
|
||||
}
|
||||
}
|
||||
|
||||
if (low<best_safe) {
|
||||
best_safe=low;
|
||||
best_unsafe=hi;
|
||||
}
|
||||
|
||||
best_normal.shape_B=col_obj->get_shape(shape_idx);
|
||||
best_normal.motion=p_motion*hi;
|
||||
best_normal.b_xform=col_obj_xform;
|
||||
best_normal.b_xform_inv=col_obj_xform.affine_inverse();
|
||||
}
|
||||
|
||||
bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal);
|
||||
print_line("CLD: "+itos(sc));
|
||||
p_closest_safe=best_safe;
|
||||
p_closest_unsafe=best_unsafe;
|
||||
|
||||
return true;
|
||||
|
||||
|
||||
if (collided && low>=r_result.travel)
|
||||
}
|
||||
|
||||
|
||||
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_user_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_user_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
collided=true;
|
||||
r_result.travel=low;
|
||||
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
|
||||
int shape_idx=space->intersection_query_subindex_results[i];
|
||||
|
||||
r_result.collider_id=col_obj->get_instance_id();
|
||||
r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
|
||||
r_result.shape=shape_idx;
|
||||
r_result.rid=col_obj->get_self();
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (collided) {
|
||||
ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false);
|
||||
r_result.normal=best_normal.best_normal;
|
||||
r_result.point=best_normal.best_contact;
|
||||
}
|
||||
r_result_count=cbk.amount;
|
||||
|
||||
return collided;
|
||||
}
|
||||
|
||||
|
||||
struct _RestCallbackData {
|
||||
|
||||
const CollisionObject2DSW *object;
|
||||
const CollisionObject2DSW *best_object;
|
||||
int shape;
|
||||
int best_shape;
|
||||
Vector2 best_contact;
|
||||
Vector2 best_normal;
|
||||
float best_len;
|
||||
};
|
||||
|
||||
static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
|
||||
|
||||
|
||||
_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
|
||||
|
||||
Vector2 contact_rel = p_point_B - p_point_A;
|
||||
float len = contact_rel.length();
|
||||
if (len <= rd->best_len)
|
||||
return;
|
||||
|
||||
rd->best_len=len;
|
||||
rd->best_contact=p_point_B;
|
||||
rd->best_normal=contact_rel/len;
|
||||
rd->best_object=rd->object;
|
||||
rd->best_shape=rd->shape;
|
||||
|
||||
}
|
||||
|
||||
|
||||
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_user_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_user_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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -46,9 +46,11 @@ public:
|
||||
|
||||
Space2DSW *space;
|
||||
|
||||
bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
|
||||
virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
virtual bool 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=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
virtual bool 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=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
|
||||
Physics2DDirectSpaceStateSW();
|
||||
};
|
||||
|
||||
@@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
|
||||
|
||||
ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
|
||||
|
||||
int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask);
|
||||
int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask);
|
||||
|
||||
if (rc==0)
|
||||
return Variant();
|
||||
@@ -163,15 +163,13 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
|
||||
Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
|
||||
|
||||
#if 0
|
||||
Set<RID> exclude;
|
||||
for(int i=0;i<p_exclude.size();i++)
|
||||
exclude.insert(p_exclude[i]);
|
||||
|
||||
|
||||
|
||||
MotionCastCollision mcc;
|
||||
|
||||
bool result = cast_motion(p_shape,p_xform,p_motion,mcc,exclude,p_user_mask);
|
||||
bool result = cast_motion(p_shape,p_xform,p_motion,0,mcc,exclude,p_user_mask);
|
||||
|
||||
if (!result)
|
||||
return Variant();
|
||||
@@ -185,7 +183,8 @@ Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix
|
||||
d["shape"]=mcc.shape;
|
||||
|
||||
return d;
|
||||
|
||||
#endif
|
||||
return Variant();
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -98,6 +98,16 @@ protected:
|
||||
|
||||
public:
|
||||
|
||||
enum ObjectTypeMask {
|
||||
TYPE_MASK_STATIC_BODY=1<<0,
|
||||
TYPE_MASK_KINEMATIC_BODY=1<<1,
|
||||
TYPE_MASK_RIGID_BODY=1<<2,
|
||||
TYPE_MASK_CHARACTER_BODY=1<<3,
|
||||
TYPE_MASK_AREA=1<<4,
|
||||
TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
|
||||
|
||||
};
|
||||
|
||||
struct RayResult {
|
||||
|
||||
Vector2 position;
|
||||
@@ -108,7 +118,7 @@ public:
|
||||
int shape;
|
||||
};
|
||||
|
||||
virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
|
||||
|
||||
struct ShapeResult {
|
||||
|
||||
@@ -119,25 +129,26 @@ public:
|
||||
|
||||
};
|
||||
|
||||
virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
|
||||
|
||||
|
||||
|
||||
struct MotionCastCollision {
|
||||
virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
|
||||
|
||||
virtual bool 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=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
|
||||
|
||||
struct ShapeRestInfo {
|
||||
|
||||
float travel; //0 to 1, if 0 then it's blocked
|
||||
Vector2 point;
|
||||
Vector2 normal;
|
||||
RID rid;
|
||||
ObjectID collider_id;
|
||||
Object *collider;
|
||||
int shape;
|
||||
Vector2 linear_velocity; //velocity at contact point
|
||||
|
||||
};
|
||||
|
||||
virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
|
||||
|
||||
|
||||
virtual bool 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=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
|
||||
|
||||
|
||||
Physics2DDirectSpaceState();
|
||||
@@ -327,8 +338,8 @@ public:
|
||||
virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0;
|
||||
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0;
|
||||
|
||||
virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
|
||||
virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
|
||||
virtual void body_set_user_mask(RID p_body, uint32_t p_mask)=0;
|
||||
virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const=0;
|
||||
|
||||
// common body variables
|
||||
enum BodyParameter {
|
||||
|
||||
Reference in New Issue
Block a user