1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 17 #ifndef BT_IDEBUG_DRAW__H 18 #define BT_IDEBUG_DRAW__H 19 20 #include "btVector3.h" 21 #include "btTransform.h" 22 23 24 ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. 25 ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. 26 ///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. 27 ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1] 28 class btIDebugDraw 29 { 30 public: 31 32 enum DebugDrawModes 33 { 34 DBG_NoDebug=0, 35 DBG_DrawWireframe = 1, 36 DBG_DrawAabb=2, 37 DBG_DrawFeaturesText=4, 38 DBG_DrawContactPoints=8, 39 DBG_NoDeactivation=16, 40 DBG_NoHelpText = 32, 41 DBG_DrawText=64, 42 DBG_ProfileTimings = 128, 43 DBG_EnableSatComparison = 256, 44 DBG_DisableBulletLCP = 512, 45 DBG_EnableCCD = 1024, 46 DBG_DrawConstraints = (1 << 11), 47 DBG_DrawConstraintLimits = (1 << 12), 48 DBG_FastWireframe = (1<<13), 49 DBG_DrawNormals = (1<<14), 50 DBG_DrawFrames = (1<<15), 51 DBG_MAX_DEBUG_DRAW_MODE 52 }; 53 ~btIDebugDraw()54 virtual ~btIDebugDraw() {}; 55 56 virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; 57 drawLine(const btVector3 & from,const btVector3 & to,const btVector3 & fromColor,const btVector3 & toColor)58 virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) 59 { 60 (void) toColor; 61 drawLine (from, to, fromColor); 62 } 63 drawSphere(btScalar radius,const btTransform & transform,const btVector3 & color)64 virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color) 65 { 66 67 btVector3 center = transform.getOrigin(); 68 btVector3 up = transform.getBasis().getColumn(1); 69 btVector3 axis = transform.getBasis().getColumn(0); 70 btScalar minTh = -SIMD_HALF_PI; 71 btScalar maxTh = SIMD_HALF_PI; 72 btScalar minPs = -SIMD_HALF_PI; 73 btScalar maxPs = SIMD_HALF_PI; 74 btScalar stepDegrees = 30.f; 75 drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false); 76 drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false ); 77 } 78 drawSphere(const btVector3 & p,btScalar radius,const btVector3 & color)79 virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) 80 { 81 btTransform tr; 82 tr.setIdentity(); 83 tr.setOrigin(p); 84 drawSphere(radius,tr,color); 85 } 86 drawTriangle(const btVector3 & v0,const btVector3 & v1,const btVector3 & v2,const btVector3 &,const btVector3 &,const btVector3 &,const btVector3 & color,btScalar alpha)87 virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha) 88 { 89 drawTriangle(v0,v1,v2,color,alpha); 90 } drawTriangle(const btVector3 & v0,const btVector3 & v1,const btVector3 & v2,const btVector3 & color,btScalar)91 virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/) 92 { 93 drawLine(v0,v1,color); 94 drawLine(v1,v2,color); 95 drawLine(v2,v0,color); 96 } 97 98 virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0; 99 100 virtual void reportErrorWarning(const char* warningString) = 0; 101 102 virtual void draw3dText(const btVector3& location,const char* textString) = 0; 103 104 virtual void setDebugMode(int debugMode) =0; 105 106 virtual int getDebugMode() const = 0; 107 drawAabb(const btVector3 & from,const btVector3 & to,const btVector3 & color)108 virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) 109 { 110 111 btVector3 halfExtents = (to-from)* 0.5f; 112 btVector3 center = (to+from) *0.5f; 113 int i,j; 114 115 btVector3 edgecoord(1.f,1.f,1.f),pa,pb; 116 for (i=0;i<4;i++) 117 { 118 for (j=0;j<3;j++) 119 { 120 pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], 121 edgecoord[2]*halfExtents[2]); 122 pa+=center; 123 124 int othercoord = j%3; 125 edgecoord[othercoord]*=-1.f; 126 pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], 127 edgecoord[2]*halfExtents[2]); 128 pb+=center; 129 130 drawLine(pa,pb,color); 131 } 132 edgecoord = btVector3(-1.f,-1.f,-1.f); 133 if (i<3) 134 edgecoord[i]*=-1.f; 135 } 136 } drawTransform(const btTransform & transform,btScalar orthoLen)137 virtual void drawTransform(const btTransform& transform, btScalar orthoLen) 138 { 139 btVector3 start = transform.getOrigin(); 140 drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); 141 drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); 142 drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); 143 } 144 145 virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, 146 const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) 147 { 148 const btVector3& vx = axis; 149 btVector3 vy = normal.cross(axis); 150 btScalar step = stepDegrees * SIMD_RADS_PER_DEG; 151 int nSteps = (int)btFabs((maxAngle - minAngle) / step); 152 if(!nSteps) nSteps = 1; 153 btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); 154 if(drawSect) 155 { 156 drawLine(center, prev, color); 157 } 158 for(int i = 1; i <= nSteps; i++) 159 { 160 btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps); 161 btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle); 162 drawLine(prev, next, color); 163 prev = next; 164 } 165 if(drawSect) 166 { 167 drawLine(center, prev, color); 168 } 169 } 170 virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, 171 btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true) 172 { 173 btVector3 vA[74]; 174 btVector3 vB[74]; 175 btVector3 *pvA = vA, *pvB = vB, *pT; 176 btVector3 npole = center + up * radius; 177 btVector3 spole = center - up * radius; 178 btVector3 arcStart; 179 btScalar step = stepDegrees * SIMD_RADS_PER_DEG; 180 const btVector3& kv = up; 181 const btVector3& iv = axis; 182 btVector3 jv = kv.cross(iv); 183 bool drawN = false; 184 bool drawS = false; 185 if(minTh <= -SIMD_HALF_PI) 186 { 187 minTh = -SIMD_HALF_PI + step; 188 drawN = true; 189 } 190 if(maxTh >= SIMD_HALF_PI) 191 { 192 maxTh = SIMD_HALF_PI - step; 193 drawS = true; 194 } 195 if(minTh > maxTh) 196 { 197 minTh = -SIMD_HALF_PI + step; 198 maxTh = SIMD_HALF_PI - step; 199 drawN = drawS = true; 200 } 201 int n_hor = (int)((maxTh - minTh) / step) + 1; 202 if(n_hor < 2) n_hor = 2; 203 btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1); 204 bool isClosed = false; 205 if(minPs > maxPs) 206 { 207 minPs = -SIMD_PI + step; 208 maxPs = SIMD_PI; 209 isClosed = true; 210 } 211 else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) 212 { 213 isClosed = true; 214 } 215 else 216 { 217 isClosed = false; 218 } 219 int n_vert = (int)((maxPs - minPs) / step) + 1; 220 if(n_vert < 2) n_vert = 2; 221 btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1); 222 for(int i = 0; i < n_hor; i++) 223 { 224 btScalar th = minTh + btScalar(i) * step_h; 225 btScalar sth = radius * btSin(th); 226 btScalar cth = radius * btCos(th); 227 for(int j = 0; j < n_vert; j++) 228 { 229 btScalar psi = minPs + btScalar(j) * step_v; 230 btScalar sps = btSin(psi); 231 btScalar cps = btCos(psi); 232 pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; 233 if(i) 234 { 235 drawLine(pvA[j], pvB[j], color); 236 } 237 else if(drawS) 238 { 239 drawLine(spole, pvB[j], color); 240 } 241 if(j) 242 { 243 drawLine(pvB[j-1], pvB[j], color); 244 } 245 else 246 { 247 arcStart = pvB[j]; 248 } 249 if((i == (n_hor - 1)) && drawN) 250 { 251 drawLine(npole, pvB[j], color); 252 } 253 254 if (drawCenter) 255 { 256 if(isClosed) 257 { 258 if(j == (n_vert-1)) 259 { 260 drawLine(arcStart, pvB[j], color); 261 } 262 } 263 else 264 { 265 if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) 266 { 267 drawLine(center, pvB[j], color); 268 } 269 } 270 } 271 } 272 pT = pvA; pvA = pvB; pvB = pT; 273 } 274 } 275 276 drawBox(const btVector3 & bbMin,const btVector3 & bbMax,const btVector3 & color)277 virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) 278 { 279 drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); 280 drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color); 281 drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color); 282 drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color); 283 drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); 284 drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); 285 drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); 286 drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); 287 drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); 288 drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); 289 drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); 290 drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); 291 } drawBox(const btVector3 & bbMin,const btVector3 & bbMax,const btTransform & trans,const btVector3 & color)292 virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) 293 { 294 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color); 295 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color); 296 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color); 297 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color); 298 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); 299 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); 300 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); 301 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); 302 drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); 303 drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); 304 drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); 305 drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); 306 } 307 drawCapsule(btScalar radius,btScalar halfHeight,int upAxis,const btTransform & transform,const btVector3 & color)308 virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) 309 { 310 int stepDegrees = 30; 311 312 btVector3 capStart(0.f,0.f,0.f); 313 capStart[upAxis] = -halfHeight; 314 315 btVector3 capEnd(0.f,0.f,0.f); 316 capEnd[upAxis] = halfHeight; 317 318 // Draw the ends 319 { 320 321 btTransform childTransform = transform; 322 childTransform.getOrigin() = transform * capStart; 323 { 324 btVector3 center = childTransform.getOrigin(); 325 btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); 326 btVector3 axis = -childTransform.getBasis().getColumn(upAxis); 327 btScalar minTh = -SIMD_HALF_PI; 328 btScalar maxTh = SIMD_HALF_PI; 329 btScalar minPs = -SIMD_HALF_PI; 330 btScalar maxPs = SIMD_HALF_PI; 331 332 drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); 333 } 334 335 336 337 } 338 339 { 340 btTransform childTransform = transform; 341 childTransform.getOrigin() = transform * capEnd; 342 { 343 btVector3 center = childTransform.getOrigin(); 344 btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3); 345 btVector3 axis = childTransform.getBasis().getColumn(upAxis); 346 btScalar minTh = -SIMD_HALF_PI; 347 btScalar maxTh = SIMD_HALF_PI; 348 btScalar minPs = -SIMD_HALF_PI; 349 btScalar maxPs = SIMD_HALF_PI; 350 drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false); 351 } 352 } 353 354 // Draw some additional lines 355 btVector3 start = transform.getOrigin(); 356 357 for (int i=0;i<360;i+=stepDegrees) 358 { 359 capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 360 capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 361 drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); 362 } 363 364 } 365 drawCylinder(btScalar radius,btScalar halfHeight,int upAxis,const btTransform & transform,const btVector3 & color)366 virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) 367 { 368 btVector3 start = transform.getOrigin(); 369 btVector3 offsetHeight(0,0,0); 370 offsetHeight[upAxis] = halfHeight; 371 int stepDegrees=30; 372 btVector3 capStart(0.f,0.f,0.f); 373 capStart[upAxis] = -halfHeight; 374 btVector3 capEnd(0.f,0.f,0.f); 375 capEnd[upAxis] = halfHeight; 376 377 for (int i=0;i<360;i+=stepDegrees) 378 { 379 capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 380 capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 381 drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color); 382 } 383 // Drawing top and bottom caps of the cylinder 384 btVector3 yaxis(0,0,0); 385 yaxis[upAxis] = btScalar(1.0); 386 btVector3 xaxis(0,0,0); 387 xaxis[(upAxis+1)%3] = btScalar(1.0); 388 drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); 389 drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); 390 } 391 drawCone(btScalar radius,btScalar height,int upAxis,const btTransform & transform,const btVector3 & color)392 virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) 393 { 394 int stepDegrees = 30; 395 btVector3 start = transform.getOrigin(); 396 397 btVector3 offsetHeight(0,0,0); 398 btScalar halfHeight = height * btScalar(0.5); 399 offsetHeight[upAxis] = halfHeight; 400 btVector3 offsetRadius(0,0,0); 401 offsetRadius[(upAxis+1)%3] = radius; 402 btVector3 offset2Radius(0,0,0); 403 offset2Radius[(upAxis+2)%3] = radius; 404 405 406 btVector3 capEnd(0.f,0.f,0.f); 407 capEnd[upAxis] = -halfHeight; 408 409 for (int i=0;i<360;i+=stepDegrees) 410 { 411 capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 412 capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius; 413 drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color); 414 } 415 416 drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color); 417 drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color); 418 drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color); 419 drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color); 420 421 // Drawing the base of the cone 422 btVector3 yaxis(0,0,0); 423 yaxis[upAxis] = btScalar(1.0); 424 btVector3 xaxis(0,0,0); 425 xaxis[(upAxis+1)%3] = btScalar(1.0); 426 drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0); 427 } 428 drawPlane(const btVector3 & planeNormal,btScalar planeConst,const btTransform & transform,const btVector3 & color)429 virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color) 430 { 431 btVector3 planeOrigin = planeNormal * planeConst; 432 btVector3 vec0,vec1; 433 btPlaneSpace1(planeNormal,vec0,vec1); 434 btScalar vecLen = 100.f; 435 btVector3 pt0 = planeOrigin + vec0*vecLen; 436 btVector3 pt1 = planeOrigin - vec0*vecLen; 437 btVector3 pt2 = planeOrigin + vec1*vecLen; 438 btVector3 pt3 = planeOrigin - vec1*vecLen; 439 drawLine(transform*pt0,transform*pt1,color); 440 drawLine(transform*pt2,transform*pt3,color); 441 } 442 flushLines()443 virtual void flushLines() 444 { 445 } 446 }; 447 448 449 #endif //BT_IDEBUG_DRAW__H 450 451