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)
{
btVector3 start = transform.getOrigin();
const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
// XY
drawLine(start-xoffs, start+yoffs, color);
drawLine(start+yoffs, start+xoffs, color);
drawLine(start+xoffs, start-yoffs, color);
drawLine(start-yoffs, start-xoffs, color);
// 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);
btVector3 center = transform.getOrigin();
btVector3 up = transform.getBasis().getColumn(1);
btVector3 axis = transform.getBasis().getColumn(0);
btScalar minTh = -SIMD_HALF_PI;
btScalar maxTh = SIMD_HALF_PI;
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
btScalar stepDegrees = 30.f;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false);
drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false );
}
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,
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 vB[74];
@@ -261,6 +249,9 @@ class btIDebugDraw
{
drawLine(npole, pvB[j], color);
}
if (drawCenter)
{
if(isClosed)
{
if(j == (n_vert-1))
@@ -276,6 +267,7 @@ class btIDebugDraw
}
}
}
}
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)
{
int stepDegrees = 30;
btVector3 capStart(0.f,0.f,0.f);
capStart[upAxis] = -halfHeight;
@@ -325,34 +319,47 @@ class btIDebugDraw
btTransform childTransform = transform;
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;
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
btVector3 start = transform.getOrigin();
capStart[(upAxis+1)%3] = radius;
capEnd[(upAxis+1)%3] = radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capStart[(upAxis+1)%3] = -radius;
capEnd[(upAxis+1)%3] = -radius;
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);
}
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)
@@ -360,11 +367,18 @@ class btIDebugDraw
btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
int stepDegrees=30;
btVector3 capStart(0.f,0.f,0.f);
capStart[upAxis] = -halfHeight;
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
btVector3 yaxis(0,0,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)
{
int stepDegrees = 30;
btVector3 start = transform.getOrigin();
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);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
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+offset2Radius),color);

7
src/LinearMath/btVector3.h
View File

@@ -289,6 +289,9 @@ public:
* x^2 + y^2 + z^2 = 1 */
SIMD_FORCE_INLINE btVector3& normalize()
{
btAssert(length() != btScalar(0));
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
// dot product first
__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
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
btVector3 norm = *this;
return norm.normalize();
#else
return *this / length();
#endif
}
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const