godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #47068 from nekomatata/cylinder-fix-margins-3.2 

[3.3] Fix Cylinder shape collision with margins when using GJK-EPA
This commit is contained in:
Rémi Verschelde 2021-03-17 11:05:46 +01:00 committed by GitHub
parent be64551922 abb5660925
commit dae319355f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 69 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
servers/physics/collision_solver_sat.cpp
View File

@ -1680,7 +1680,7 @@ static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_t
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CapsuleShapeSW, CylinderShapeSW, withMargin>::test_contact_points; CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CapsuleShapeSW, CylinderShapeSW, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
return; return;
} }
@ -1854,7 +1854,7 @@ static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, CylinderShapeSW, withMargin>::test_contact_points; CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, CylinderShapeSW, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
return; return;
} }
@ -1871,7 +1871,7 @@ static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transfo
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, ConvexPolygonShapeSW, withMargin>::test_contact_points; CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, ConvexPolygonShapeSW, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
return; return;
} }

95
servers/physics/gjk_epa.cpp
View File

@ -111,25 +111,60 @@ struct MinkowskiDiff {
Transform transform_A; Transform transform_A;
Transform transform_B; Transform transform_B;
real_t margin_A = 0.0;
real_t margin_B = 0.0;
Vector3 (*get_support)(const ShapeSW*, const Vector3&, real_t);
void Initialize(const ShapeSW* shape0, const Transform& wtrs0, const real_t margin0,
const ShapeSW* shape1, const Transform& wtrs1, const real_t margin1) {
m_shapes[0] = shape0;
m_shapes[1] = shape1;
transform_A = wtrs0;
transform_B = wtrs1;
margin_A = margin0;
margin_B = margin1;
if ((margin0 > 0.0) || (margin1 > 0.0)) {
get_support = get_support_with_margin;
} else {
get_support = get_support_without_margin;
}
}
static Vector3 get_support_without_margin(const ShapeSW* p_shape, const Vector3& p_dir, real_t p_margin) {
return p_shape->get_support(p_dir.normalized());
}
static Vector3 get_support_with_margin(const ShapeSW* p_shape, const Vector3& p_dir, real_t p_margin) {
Vector3 local_dir_norm = p_dir;
if (local_dir_norm.length_squared() < CMP_EPSILON2) {
local_dir_norm = Vector3(-1.0, -1.0, -1.0);
}
local_dir_norm.normalize();
return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm;
}
// i wonder how this could be sped up... if it can // i wonder how this could be sped up... if it can
_FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const { _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const {
return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) ); return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A));
} }
_FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const { _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const {
return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) ); return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B));
} }
_FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const { _FORCE_INLINE_ Vector3 Support (const Vector3& d) const {
return ( Support0 ( d )-Support1 ( -d ) ); return (Support0(d) - Support1(-d));
} }
_FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const {
{ if (index) {
if ( index ) return Support1(d);
return ( Support1 ( d ) ); } else {
else return Support0(d);
return ( Support0 ( d ) ); }
} }
}; };
@ -822,21 +857,17 @@ struct GJK
}; };
// //
static void Initialize( const ShapeSW* shape0,const Transform& wtrs0, static void Initialize( const ShapeSW* shape0, const Transform& wtrs0, real_t margin0,
const ShapeSW* shape1,const Transform& wtrs1, const ShapeSW* shape1, const Transform& wtrs1, real_t margin1,
sResults& results, sResults& results,
tShape& shape, tShape& shape)
bool withmargins)
{ {
/* Results */ /* Results */
results.witnesses[0] = results.witnesses[0] = Vector3(0,0,0);
results.witnesses[1] = Vector3(0,0,0); results.witnesses[1] = Vector3(0,0,0);
results.status = sResults::Separated; results.status = sResults::Separated;
/* Shape */ /* Shape */
shape.m_shapes[0] = shape0; shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1);
shape.m_shapes[1] = shape1;
shape.transform_A = wtrs0;
shape.transform_B = wtrs1;
} }
@ -851,13 +882,15 @@ struct GJK
// //
bool Distance( const ShapeSW* shape0, bool Distance( const ShapeSW* shape0,
const Transform& wtrs0, const Transform& wtrs0,
const ShapeSW* shape1, real_t margin0,
const ShapeSW* shape1,
const Transform& wtrs1, const Transform& wtrs1,
real_t margin1,
const Vector3& guess, const Vector3& guess,
sResults& results) sResults& results)
{ {
tShape shape; tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
GJK gjk; GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid) if(gjk_status==GJK::eStatus::Valid)
@ -889,14 +922,16 @@ bool Distance( const ShapeSW* shape0,
// //
bool Penetration( const ShapeSW* shape0, bool Penetration( const ShapeSW* shape0,
const Transform& wtrs0, const Transform& wtrs0,
const ShapeSW* shape1, real_t margin0,
const ShapeSW* shape1,
const Transform& wtrs1, const Transform& wtrs1,
const Vector3& guess, real_t margin1,
const Vector3& guess,
sResults& results sResults& results
) )
{ {
tShape shape; tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
GJK gjk; GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
switch(gjk_status) switch(gjk_status)
@ -957,7 +992,7 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra
GjkEpa2::sResults res; GjkEpa2::sResults res;
if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) {
r_result_A = res.witnesses[0]; r_result_A = res.witnesses[0];
r_result_B = res.witnesses[1]; r_result_B = res.witnesses[1];
@ -967,11 +1002,11 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra
return false; return false;
} }
bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) { bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
GjkEpa2::sResults res; GjkEpa2::sResults res;
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
if (p_result_callback) { if (p_result_callback) {
if (p_swap) if (p_swap)
p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);

2
servers/physics/gjk_epa.h
View File

@ -34,7 +34,7 @@
#include "collision_solver_sw.h" #include "collision_solver_sw.h"
#include "shape_sw.h" #include "shape_sw.h"
bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0);
bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
#endif #endif