Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Cylinder shape collision with margins when using GJK-EPA #47067

Merged
merged 1 commit into from
Mar 17, 2021
Merged

Fix Cylinder shape collision with margins when using GJK-EPA #47067

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions servers/physics_3d/collision_solver_3d_sat.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1630,7 +1630,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points;

// 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;
}

Expand Down Expand Up @@ -1805,7 +1805,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points;

// 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;
}

Expand All @@ -1822,7 +1822,7 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points;

// 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;
}

Expand Down
95 changes: 64 additions & 31 deletions servers/physics_3d/gjk_epa.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,26 +110,60 @@ struct MinkowskiDiff {
Transform transform_A;
Transform transform_B;

real_t margin_A = 0.0;
real_t margin_B = 0.0;

Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t);

void Initialize(const Shape3DSW* shape0, const Transform& wtrs0, const real_t margin0,
const Shape3DSW* 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 Shape3DSW* 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 Shape3DSW* 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
_FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const {
return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) );
_FORCE_INLINE_ Vector3 Support0(const Vector3& d) const {
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 {
return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) );
_FORCE_INLINE_ Vector3 Support1(const Vector3& d) const {
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 {
return ( Support0 ( d )-Support1 ( -d ) );
_FORCE_INLINE_ Vector3 Support (const Vector3& d) const {
return (Support0(d) - Support1(-d));
}

_FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const
{
if ( index ) {
return ( Support1 ( d ) );
_FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const {
if (index) {
return Support1(d);
} else {
return ( Support0 ( d ) );
}
return Support0(d);
}
}
};

Expand Down Expand Up @@ -828,22 +862,17 @@ struct GJK
};

//
static void Initialize( const Shape3DSW* shape0,const Transform& wtrs0,
const Shape3DSW* shape1,const Transform& wtrs1,
static void Initialize( const Shape3DSW* shape0, const Transform& wtrs0, real_t margin0,
const Shape3DSW* shape1, const Transform& wtrs1, real_t margin1,
sResults& results,
tShape& shape,
bool withmargins)
tShape& shape)
{
/* Results */
results.witnesses[0] =
results.witnesses[1] = Vector3(0,0,0);
results.witnesses[0] = Vector3(0,0,0);
results.witnesses[1] = Vector3(0,0,0);
results.status = sResults::Separated;
/* Shape */
shape.m_shapes[0] = shape0;
shape.m_shapes[1] = shape1;
shape.transform_A = wtrs0;
shape.transform_B = wtrs1;

shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1);
}


Expand All @@ -857,13 +886,15 @@ struct GJK
//
bool Distance( const Shape3DSW* shape0,
const Transform& wtrs0,
const Shape3DSW* shape1,
real_t margin0,
const Shape3DSW* shape1,
const Transform& wtrs1,
real_t margin1,
const Vector3& guess,
sResults& results)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid)
Expand Down Expand Up @@ -896,14 +927,16 @@ bool Distance( const Shape3DSW* shape0,
//
bool Penetration( const Shape3DSW* shape0,
const Transform& wtrs0,
const Shape3DSW* shape1,
real_t margin0,
const Shape3DSW* shape1,
const Transform& wtrs1,
const Vector3& guess,
real_t margin1,
const Vector3& guess,
sResults& results
)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
switch(gjk_status)
Expand Down Expand Up @@ -963,7 +996,7 @@ bool Penetration( const Shape3DSW* shape0,
bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
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_B = res.witnesses[1];
return true;
Expand All @@ -972,10 +1005,10 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t
return false;
}

bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
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_swap) {
p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_3d/gjk_epa.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
#include "collision_solver_3d_sw.h"
#include "shape_3d_sw.h"

bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false);
bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);

#endif