summaryrefslogtreecommitdiff
path: root/servers/physics_3d/gjk_epa.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/gjk_epa.cpp')
-rw-r--r--servers/physics_3d/gjk_epa.cpp104
1 files changed, 71 insertions, 33 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index db37f261ce..d99a2532f8 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -123,10 +123,12 @@ struct MinkowskiDiff {
_FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const
{
- if ( index )
+ if ( index ) {
return ( Support1 ( d ) );
- else
+ } else {
return ( Support0 ( d ) );
+
+}
}
};
@@ -278,7 +280,9 @@ struct GJK
m_free[m_nfree++] = cs.c[i];
}
}
- if(mask==15) m_status=eStatus::Inside;
+ if(mask==15) { m_status=eStatus::Inside;
+
+}
}
else
{/* Return old simplex */
@@ -307,10 +311,14 @@ struct GJK
Vector3 axis=Vector3(0,0,0);
axis[i]=1;
appendvertice(*m_simplex, axis);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
appendvertice(*m_simplex,-axis);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
}
}
@@ -326,10 +334,14 @@ struct GJK
if(p.length_squared()>0)
{
appendvertice(*m_simplex, p);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
appendvertice(*m_simplex,-p);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
}
}
@@ -342,10 +354,14 @@ struct GJK
if(n.length_squared()>0)
{
appendvertice(*m_simplex,n);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
appendvertice(*m_simplex,-n);
- if(EncloseOrigin()) return(true);
+ if(EncloseOrigin()) { return(true);
+
+}
removevertice(*m_simplex);
}
}
@@ -354,8 +370,10 @@ struct GJK
{
if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
m_simplex->c[1]->w-m_simplex->c[3]->w,
- m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+ m_simplex->c[2]->w-m_simplex->c[3]->w))>0) {
return(true);
+
+}
}
break;
}
@@ -513,16 +531,16 @@ struct GJK
};
struct sList
{
- sFace* root;
- U count;
- sList() : root(nullptr),count(0) {}
+ sFace* root = nullptr;
+ U count = 0;
+ sList() {}
};
struct sHorizon
{
- sFace* cf;
- sFace* ff;
- U nf;
- sHorizon() : cf(nullptr),ff(nullptr),nf(0) {}
+ sFace* cf = nullptr;
+ sFace* ff = nullptr;
+ U nf = 0;
+ sHorizon() {}
};
struct eStatus { enum _ {
Valid,
@@ -561,15 +579,23 @@ struct GJK
{
face->l[0] = nullptr;
face->l[1] = list.root;
- if(list.root) list.root->l[0]=face;
+ if(list.root) { list.root->l[0]=face;
+
+}
list.root = face;
++list.count;
}
static inline void remove(sList& list,sFace* face)
{
- if(face->l[1]) face->l[1]->l[0]=face->l[0];
- if(face->l[0]) face->l[0]->l[1]=face->l[1];
- if(face==list.root) list.root=face->l[1];
+ if(face->l[1]) { face->l[1]->l[0]=face->l[0];
+
+}
+ if(face->l[0]) { face->l[0]->l[1]=face->l[1];
+
+}
+ if(face==list.root) { list.root=face->l[1];
+
+}
--list.count;
}
@@ -650,7 +676,9 @@ struct GJK
remove(m_hull,best);
append(m_stock,best);
best=findbest();
- if(best->p>=outer.p) outer=*best;
+ if(best->p>=outer.p) { outer=*best;
+
+}
} else { m_status=eStatus::InvalidHull;break; }
} else { m_status=eStatus::AccuraryReached;break; }
} else { m_status=eStatus::OutOfVertices;break; }
@@ -679,10 +707,12 @@ struct GJK
m_status = eStatus::FallBack;
m_normal = -guess;
const real_t nl=m_normal.length();
- if(nl>0)
+ if(nl>0) {
m_normal = m_normal/nl;
- else
+ } else {
m_normal = Vector3(1,0,0);
+
+}
m_depth = 0;
m_result.rank=1;
m_result.c[0]=simplex.c[0];
@@ -716,8 +746,12 @@ struct GJK
if(forced||(face->d>=-EPA_PLANE_EPS))
{
return(face);
- } else m_status=eStatus::NonConvex;
- } else m_status=eStatus::Degenerated;
+ } else { m_status=eStatus::NonConvex;
+
+}
+ } else { m_status=eStatus::Degenerated;
+
+}
remove(m_hull,face);
append(m_stock,face);
return(nullptr);
@@ -758,7 +792,9 @@ struct GJK
if(nf)
{
bind(nf,0,f,e);
- if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+ if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf;
+
+}
horizon.cf=nf;
++horizon.nf;
return(true);
@@ -847,6 +883,7 @@ bool Distance( const Shape3DSW* shape0,
}
}
+
//
bool Penetration( const Shape3DSW* shape0,
const Transform& wtrs0,
@@ -879,7 +916,9 @@ bool Penetration( const Shape3DSW* shape0,
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return(true);
- } else results.status=sResults::EPA_Failed;
+ } else { results.status=sResults::EPA_Failed;
+
+}
}
break;
case GJK::eStatus::Failed:
@@ -891,6 +930,7 @@ bool Penetration( const Shape3DSW* shape0,
}
+
/* Symbols cleanup */
#undef GJK_MAX_ITERATIONS
@@ -915,11 +955,9 @@ bool Penetration( const Shape3DSW* shape0,
/* clang-format on */
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)) {
-
r_result_A = res.witnesses[0];
r_result_B = res.witnesses[1];
return true;
@@ -929,15 +967,15 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t
}
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) {
-
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 (p_result_callback) {
- if (p_swap)
+ if (p_swap) {
p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
- else
+ } else {
p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
+ }
}
return true;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 07:36:05 +0000