From f75ae815d51571287892d77414d45e15bfdb849b Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Mon, 5 Jan 2015 23:00:35 -0300 Subject: [PATCH] -CCD in 3D physics was not working (code was not even there!) re-added, fixes 1067 --- servers/physics/body_pair_sw.cpp | 62 +++++++++++++++++++++++++- servers/physics/body_pair_sw.h | 1 + servers/physics_2d/body_pair_2d_sw.cpp | 2 +- 3 files changed, 63 insertions(+), 2 deletions(-) diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 2e66c9e27bf..5847b942fbd 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -172,6 +172,53 @@ void BodyPairSW::validate_contacts() { } } + +bool BodyPairSW::_test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B) { + + + + Vector3 motion = p_A->get_linear_velocity()*p_step; + real_t mlen = motion.length(); + if (mlenget_shape(p_shape_A)->project_range(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; + Vector3 s=p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); + Vector3 from = p_xform_A.xform(s); + Vector3 to = from + motion; + + Transform from_inv = p_xform_B.affine_inverse(); + + Vector3 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box + Vector3 local_to = from_inv.xform(to); + + Vector3 rpos,rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) { + return false; + } + + //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough + Vector3 hitpos = p_xform_B.xform(rpos); + + float newlen = hitpos.distance_to(from)-(max-min)*0.01; + p_A->set_linear_velocity((mnormal*newlen)/p_step); + + return true; +} + + bool BodyPairSW::setup(float p_step) { //cannot collide @@ -198,8 +245,21 @@ bool BodyPairSW::setup(float p_step) { bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis); this->collided=collided; - if (!collided) + + if (!collided) { + + //test ccd (currently just a raycast) + + if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) { + _test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B); + } + + if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) { + _test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A); + } + return false; + } diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index 937c295c638..e64464e2c19 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -82,6 +82,7 @@ class BodyPairSW : public ConstraintSW { void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B); void validate_contacts(); + bool _test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B); SpaceSW *space; diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 9abdd01791b..c4d6abe5acc 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -190,7 +190,7 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat 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 + 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; }