diff options
Diffstat (limited to 'servers/physics_3d/gjk_epa.cpp')
| -rw-r--r-- | servers/physics_3d/gjk_epa.cpp | 85 |
1 files changed, 62 insertions, 23 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 7f7fc0f55b..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; } @@ -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); @@ -880,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: @@ -933,10 +971,11 @@ bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform & 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; } |