summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h')
-rw-r--r--thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h224
1 files changed, 105 insertions, 119 deletions
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h b/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h
index 60f06510ad..6a1ee6dcf9 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h
@@ -27,52 +27,44 @@ subject to the following restrictions:
#include "btBoxCollision.h"
-
-
-
-
#define PLANEDIREPSILON 0.0000001f
#define PARALELENORMALS 0.000001f
-
-#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+#define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
/// Calc a plane from a triangle edge an a normal. plane is a vec4f
-SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane)
+SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane)
{
- btVector3 planenormal = (e2-e1).cross(normal);
+ btVector3 planenormal = (e2 - e1).cross(normal);
planenormal.normalize();
- plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
+ plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal));
}
-
-
//***************** SEGMENT and LINE FUNCTIONS **********************************///
/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
*/
SIMD_FORCE_INLINE void bt_closest_point_on_segment(
- btVector3 & cp, const btVector3 & v,
- const btVector3 &e1,const btVector3 &e2)
+ btVector3 &cp, const btVector3 &v,
+ const btVector3 &e1, const btVector3 &e2)
{
- btVector3 n = e2-e1;
- cp = v - e1;
- btScalar _scalar = cp.dot(n)/n.dot(n);
- if(_scalar <0.0f)
+ btVector3 n = e2 - e1;
+ cp = v - e1;
+ btScalar _scalar = cp.dot(n) / n.dot(n);
+ if (_scalar < 0.0f)
{
- cp = e1;
+ cp = e1;
}
- else if(_scalar >1.0f)
+ else if (_scalar > 1.0f)
{
- cp = e2;
+ cp = e2;
}
else
{
- cp = _scalar*n + e1;
+ cp = _scalar * n + e1;
}
}
-
//! line plane collision
/*!
*\return
@@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment(
*/
SIMD_FORCE_INLINE int bt_line_plane_collision(
- const btVector4 & plane,
- const btVector3 & vDir,
- const btVector3 & vPoint,
- btVector3 & pout,
+ const btVector4 &plane,
+ const btVector3 &vDir,
+ const btVector3 &vPoint,
+ btVector3 &pout,
btScalar &tparam,
btScalar tmin, btScalar tmax)
{
-
btScalar _dotdir = vDir.dot(plane);
- if(btFabs(_dotdir)<PLANEDIREPSILON)
+ if (btFabs(_dotdir) < PLANEDIREPSILON)
{
tparam = tmax;
- return 0;
+ return 0;
}
- btScalar _dis = bt_distance_point_plane(plane,vPoint);
- char returnvalue = _dis<0.0f? 2:1;
- tparam = -_dis/_dotdir;
+ btScalar _dis = bt_distance_point_plane(plane, vPoint);
+ char returnvalue = _dis < 0.0f ? 2 : 1;
+ tparam = -_dis / _dotdir;
- if(tparam<tmin)
+ if (tparam < tmin)
{
returnvalue = 0;
tparam = tmin;
}
- else if(tparam>tmax)
+ else if (tparam > tmax)
{
returnvalue = 0;
tparam = tmax;
}
- pout = tparam*vDir + vPoint;
+ pout = tparam * vDir + vPoint;
return returnvalue;
}
-
//! Find closest points on segments
SIMD_FORCE_INLINE void bt_segment_collision(
- const btVector3 & vA1,
- const btVector3 & vA2,
- const btVector3 & vB1,
- const btVector3 & vB2,
- btVector3 & vPointA,
- btVector3 & vPointB)
+ const btVector3 &vA1,
+ const btVector3 &vA2,
+ const btVector3 &vB1,
+ const btVector3 &vB2,
+ btVector3 &vPointA,
+ btVector3 &vPointB)
{
- btVector3 AD = vA2 - vA1;
- btVector3 BD = vB2 - vB1;
- btVector3 N = AD.cross(BD);
- btScalar tp = N.length2();
-
- btVector4 _M;//plane
-
- if(tp<SIMD_EPSILON)//ARE PARALELE
- {
- //project B over A
- bool invert_b_order = false;
- _M[0] = vB1.dot(AD);
- _M[1] = vB2.dot(AD);
-
- if(_M[0]>_M[1])
- {
- invert_b_order = true;
- BT_SWAP_NUMBERS(_M[0],_M[1]);
- }
- _M[2] = vA1.dot(AD);
- _M[3] = vA2.dot(AD);
- //mid points
- N[0] = (_M[0]+_M[1])*0.5f;
- N[1] = (_M[2]+_M[3])*0.5f;
-
- if(N[0]<N[1])
- {
- if(_M[1]<_M[2])
- {
- vPointB = invert_b_order?vB1:vB2;
- vPointA = vA1;
- }
- else if(_M[1]<_M[3])
- {
- vPointB = invert_b_order?vB1:vB2;
- bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
- }
- else
- {
- vPointA = vA2;
- bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
- }
- }
- else
- {
- if(_M[3]<_M[0])
- {
- vPointB = invert_b_order?vB2:vB1;
- vPointA = vA2;
- }
- else if(_M[3]<_M[1])
- {
- vPointA = vA2;
- bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
- }
- else
- {
- vPointB = invert_b_order?vB1:vB2;
- bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
- }
- }
- return;
- }
-
- N = N.cross(BD);
- _M.setValue(N[0],N[1],N[2],vB1.dot(N));
+ btVector3 AD = vA2 - vA1;
+ btVector3 BD = vB2 - vB1;
+ btVector3 N = AD.cross(BD);
+ btScalar tp = N.length2();
- // get point A as the plane collision point
- bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
-
- /*Closest point on segment*/
- vPointB = vPointA - vB1;
- tp = vPointB.dot(BD);
- tp/= BD.dot(BD);
- tp = BT_CLAMP(tp,0.0f,1.0f);
+ btVector4 _M; //plane
- vPointB = tp*BD + vB1;
-}
+ if (tp < SIMD_EPSILON) //ARE PARALELE
+ {
+ //project B over A
+ bool invert_b_order = false;
+ _M[0] = vB1.dot(AD);
+ _M[1] = vB2.dot(AD);
+
+ if (_M[0] > _M[1])
+ {
+ invert_b_order = true;
+ BT_SWAP_NUMBERS(_M[0], _M[1]);
+ }
+ _M[2] = vA1.dot(AD);
+ _M[3] = vA2.dot(AD);
+ //mid points
+ N[0] = (_M[0] + _M[1]) * 0.5f;
+ N[1] = (_M[2] + _M[3]) * 0.5f;
+
+ if (N[0] < N[1])
+ {
+ if (_M[1] < _M[2])
+ {
+ vPointB = invert_b_order ? vB1 : vB2;
+ vPointA = vA1;
+ }
+ else if (_M[1] < _M[3])
+ {
+ vPointB = invert_b_order ? vB1 : vB2;
+ bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
+ }
+ else
+ {
+ vPointA = vA2;
+ bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
+ }
+ }
+ else
+ {
+ if (_M[3] < _M[0])
+ {
+ vPointB = invert_b_order ? vB2 : vB1;
+ vPointA = vA2;
+ }
+ else if (_M[3] < _M[1])
+ {
+ vPointA = vA2;
+ bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
+ }
+ else
+ {
+ vPointB = invert_b_order ? vB1 : vB2;
+ bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
+ }
+ }
+ return;
+ }
+ N = N.cross(BD);
+ _M.setValue(N[0], N[1], N[2], vB1.dot(N));
+ // get point A as the plane collision point
+ bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1));
+ /*Closest point on segment*/
+ vPointB = vPointA - vB1;
+ tp = vPointB.dot(BD);
+ tp /= BD.dot(BD);
+ tp = BT_CLAMP(tp, 0.0f, 1.0f);
+ vPointB = tp * BD + vB1;
+}
-#endif // GIM_VECTOR_H_INCLUDED
+#endif // GIM_VECTOR_H_INCLUDED
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 09:19:22 +0000