Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

prettify the debug wireframe drawing of sphere, capsule, cylinder and cone

Browse Source 
add an btAssert in normalize of btVector3 and 'normalized' always use 'normalize'
This commit is contained in:
erwin.coumans
2013-09-17 02:01:14 +00:00
parent 5f5d601ebf
commit 858c890c96
2 changed files with 85 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
src/LinearMath/btIDebugDraw.h
View File

@@ -62,29 +62,17 @@ class btIDebugDraw
virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{ {
btVector3 start = transform.getOrigin();
const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); btVector3 center = transform.getOrigin();
const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); btVector3 up = transform.getBasis().getColumn(1);
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); btVector3 axis = transform.getBasis().getColumn(0);
btScalar minTh = -SIMD_HALF_PI;
// XY btScalar maxTh = SIMD_HALF_PI;
drawLine(start-xoffs, start+yoffs, color); btScalar minPs = -SIMD_HALF_PI;
drawLine(start+yoffs, start+xoffs, color); btScalar maxPs = SIMD_HALF_PI;
drawLine(start+xoffs, start-yoffs, color); btScalar stepDegrees = 30.f;
drawLine(start-yoffs, start-xoffs, color); drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false);
drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false );
// XZ
drawLine(start-xoffs, start+zoffs, color);
drawLine(start+zoffs, start+xoffs, color);
drawLine(start+xoffs, start-zoffs, color);
drawLine(start-zoffs, start-xoffs, color);
// YZ
drawLine(start-yoffs, start+zoffs, color);
drawLine(start+zoffs, start+yoffs, color);
drawLine(start+yoffs, start-zoffs, color);
drawLine(start-zoffs, start-yoffs, color);
} }
virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
@@ -179,7 +167,7 @@ class btIDebugDraw
} }
} }
virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f)) btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true)
{ {
btVector3 vA[74]; btVector3 vA[74];
btVector3 vB[74]; btVector3 vB[74];
@@ -261,6 +249,9 @@ class btIDebugDraw
{ {
drawLine(npole, pvB[j], color); drawLine(npole, pvB[j], color);
} }
if (drawCenter)
{
if(isClosed) if(isClosed)
{ {
if(j == (n_vert-1)) if(j == (n_vert-1))
@@ -276,6 +267,7 @@ class btIDebugDraw
} }
} }
} }
}
pT = pvA; pvA = pvB; pvB = pT; pT = pvA; pvA = pvB; pvB = pT;
} }
} }
@@ -314,6 +306,8 @@ class btIDebugDraw
virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
{ {
int stepDegrees = 30;
btVector3 capStart(0.f,0.f,0.f); btVector3 capStart(0.f,0.f,0.f);
capStart[upAxis] = -halfHeight; capStart[upAxis] = -halfHeight;
@@ -325,34 +319,47 @@ class btIDebugDraw
btTransform childTransform = transform; btTransform childTransform = transform;
childTransform.getOrigin() = transform * capStart; childTransform.getOrigin() = transform * capStart;
drawSphere(radius, childTransform, color); {
btVector3 center = childTransform.getOrigin();
btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
btVector3 axis = -childTransform.getBasis().getColumn(upAxis);
btScalar minTh = -SIMD_HALF_PI;
btScalar maxTh = SIMD_HALF_PI;
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
}
} }
{ {
btTransform childTransform = transform; btTransform childTransform = transform;
childTransform.getOrigin() = transform * capEnd; childTransform.getOrigin() = transform * capEnd;
drawSphere(radius, childTransform, color); {
btVector3 center = childTransform.getOrigin();
btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
btVector3 axis = childTransform.getBasis().getColumn(upAxis);
btScalar minTh = -SIMD_HALF_PI;
btScalar maxTh = SIMD_HALF_PI;
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
}
} }
// Draw some additional lines // Draw some additional lines
btVector3 start = transform.getOrigin(); btVector3 start = transform.getOrigin();
capStart[(upAxis+1)%3] = radius; for (int i=0;i<360;i+=stepDegrees)
capEnd[(upAxis+1)%3] = radius; {
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capStart[(upAxis+1)%3] = -radius; capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+1)%3] = -radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
}
capStart[(upAxis+1)%3] = 0.f;
capEnd[(upAxis+1)%3] = 0.f;
capStart[(upAxis+2)%3] = radius;
capEnd[(upAxis+2)%3] = radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capStart[(upAxis+2)%3] = -radius;
capEnd[(upAxis+2)%3] = -radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
} }
virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
@@ -360,11 +367,18 @@ class btIDebugDraw
btVector3 start = transform.getOrigin(); btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0); btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight; offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0); int stepDegrees=30;
offsetRadius[(upAxis+1)%3] = radius; btVector3 capStart(0.f,0.f,0.f);
drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); capStart[upAxis] = -halfHeight;
drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); btVector3 capEnd(0.f,0.f,0.f);
capEnd[upAxis] = halfHeight;
for (int i=0;i<360;i+=stepDegrees)
{
capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
}
// Drawing top and bottom caps of the cylinder // Drawing top and bottom caps of the cylinder
btVector3 yaxis(0,0,0); btVector3 yaxis(0,0,0);
yaxis[upAxis] = btScalar(1.0); yaxis[upAxis] = btScalar(1.0);
@@ -376,16 +390,28 @@ class btIDebugDraw
virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
{ {
int stepDegrees = 30;
btVector3 start = transform.getOrigin(); btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0); btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5); btScalar halfHeight = height * btScalar(0.5);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0); btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius; offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0); btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius; offset2Radius[(upAxis+2)%3] = radius;
btVector3 capEnd(0.f,0.f,0.f);
capEnd[upAxis] = -halfHeight;
for (int i=0;i<360;i+=stepDegrees)
{
capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color);
}
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color); drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color);

7
src/LinearMath/btVector3.h
View File

@@ -289,6 +289,9 @@ public:
* x^2 + y^2 + z^2 = 1 */ * x^2 + y^2 + z^2 = 1 */
SIMD_FORCE_INLINE btVector3& normalize() SIMD_FORCE_INLINE btVector3& normalize()
{ {
btAssert(length() != btScalar(0));
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
// dot product first // dot product first
__m128 vd = _mm_mul_ps(mVec128, mVec128); __m128 vd = _mm_mul_ps(mVec128, mVec128);
@@ -940,13 +943,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
SIMD_FORCE_INLINE btVector3 btVector3::normalized() const SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
{ {
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
btVector3 norm = *this; btVector3 norm = *this;
return norm.normalize(); return norm.normalize();
#else
return *this / length();
#endif
} }
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const