diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btIDebugDraw.h')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btIDebugDraw.h | 138 |
1 files changed, 82 insertions, 56 deletions
diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index a00d7763a75..de97c3f87fd 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -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,18 +249,22 @@ class btIDebugDraw { drawLine(npole, pvB[j], color); } - if(isClosed) + + if (drawCenter) { - if(j == (n_vert-1)) + if(isClosed) { - drawLine(arcStart, pvB[j], color); + if(j == (n_vert-1)) + { + drawLine(arcStart, pvB[j], color); + } } - } - else - { - if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + else { - drawLine(center, pvB[j], color); + if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + { + drawLine(center, pvB[j], color); + } } } } @@ -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; - 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); + 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); + } + } 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); |