Fix Cylinder shape collision with margins when using GJK-EPA
In the case of falling back to GJK-EPA algorithm to generate cylinder contact points, margins were never taken into account. This fixes the depenetration phase for kinematic bodies and allows consistent floor detection for cylinder shapes.
This commit is contained in:
PouleyKetchoupp
parent
3ddab1c9d1
commit
abb5660925
|
|
@ -1635,7 +1635,7 @@ static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_t
|
|||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CapsuleShapeSW, CylinderShapeSW, 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;
|
||||
}
|
||||
|
||||
|
|
@ -1809,7 +1809,7 @@ static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_
|
|||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, CylinderShapeSW, 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;
|
||||
}
|
||||
|
||||
|
|
@ -1826,7 +1826,7 @@ static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transfo
|
|||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, ConvexPolygonShapeSW, 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -111,25 +111,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 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
|
||||
_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 ) );
|
||||
else
|
||||
return ( Support0 ( d ) );
|
||||
_FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const {
|
||||
if (index) {
|
||||
return Support1(d);
|
||||
} else {
|
||||
return Support0(d);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -822,21 +857,17 @@ struct GJK
|
|||
};
|
||||
|
||||
//
|
||||
static void Initialize( const ShapeSW* shape0,const Transform& wtrs0,
|
||||
const ShapeSW* shape1,const Transform& wtrs1,
|
||||
static void Initialize( const ShapeSW* shape0, const Transform& wtrs0, real_t margin0,
|
||||
const ShapeSW* 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);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -851,13 +882,15 @@ struct GJK
|
|||
//
|
||||
bool Distance( const ShapeSW* shape0,
|
||||
const Transform& wtrs0,
|
||||
const ShapeSW* shape1,
|
||||
real_t margin0,
|
||||
const ShapeSW* 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)
|
||||
|
|
@ -889,14 +922,16 @@ bool Distance( const ShapeSW* shape0,
|
|||
//
|
||||
bool Penetration( const ShapeSW* shape0,
|
||||
const Transform& wtrs0,
|
||||
const ShapeSW* shape1,
|
||||
real_t margin0,
|
||||
const ShapeSW* 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)
|
||||
|
|
@ -957,7 +992,7 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra
|
|||
|
||||
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];
|
||||
|
|
@ -967,11 +1002,11 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra
|
|||
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;
|
||||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
#include "collision_solver_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);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in a new issue