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