diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c6fb5440f..cffe3a41f 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -1227,50 +1227,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 				btScalar halfHeight = capsuleShape->getHalfHeight();
 
 				int upAxis = capsuleShape->getUpAxis();
-
-
-				btVector3 capStart(0.f,0.f,0.f);
-				capStart[upAxis] = -halfHeight;
-
-				btVector3 capEnd(0.f,0.f,0.f);
-				capEnd[upAxis] = halfHeight;
-
-				// Draw the ends
-				{
-
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capStart;
-					getDebugDrawer()->drawSphere(radius, childTransform, color);
-				}
-
-				{
-					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * capEnd;
-					getDebugDrawer()->drawSphere(radius, childTransform, color);
-				}
-
-				// Draw some additional lines
-				btVector3 start = worldTransform.getOrigin();
-
-
-				capStart[(upAxis+1)%3] = radius;
-				capEnd[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+1)%3] = -radius;
-				capEnd[(upAxis+1)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.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;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-				capStart[(upAxis+2)%3] = -radius;
-				capEnd[(upAxis+2)%3] = -radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-
-
+				getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
 				break;
 			}
 		case CONE_SHAPE_PROXYTYPE:
@@ -1278,30 +1235,10 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
 				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-				btVector3 start = worldTransform.getOrigin();
 
 				int upAxis= coneShape->getConeUpIndex();
-
-
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = height * btScalar(0.5);
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				btVector3	offset2Radius(0,0,0);
-				offset2Radius[(upAxis+2)%3] = radius;
-
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
-
-				// Drawing the base of the cone
-				btVector3 yaxis(0,0,0);
-				yaxis[upAxis] = btScalar(1.0);
-				btVector3 xaxis(0,0,0);
-				xaxis[(upAxis+1)%3] = btScalar(1.0);
-				getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
-		break;
+				getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
+				break;
 
 			}
 		case CYLINDER_SHAPE_PROXYTYPE:
@@ -1310,21 +1247,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 				int upAxis = cylinder->getUpAxis();
 				btScalar radius = cylinder->getRadius();
 				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-				btVector3 start = worldTransform.getOrigin();
-				btVector3	offsetHeight(0,0,0);
-				offsetHeight[upAxis] = halfHeight;
-				btVector3	offsetRadius(0,0,0);
-				offsetRadius[(upAxis+1)%3] = radius;
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-
-				// Drawing top and bottom caps of the cylinder
-				btVector3 yaxis(0,0,0);
-				yaxis[upAxis] = btScalar(1.0);
-				btVector3 xaxis(0,0,0);
-				xaxis[(upAxis+1)%3] = btScalar(1.0);
-				getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
-				getDebugDrawer()->drawArc(start+worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+				getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
 				break;
 			}
 
@@ -1333,16 +1256,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 				const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
 				btScalar planeConst = staticPlaneShape->getPlaneConstant();
 				const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-				btVector3 planeOrigin = planeNormal * planeConst;
-				btVector3 vec0,vec1;
-				btPlaneSpace1(planeNormal,vec0,vec1);
-				btScalar vecLen = 100.f;
-				btVector3 pt0 = planeOrigin + vec0*vecLen;
-				btVector3 pt1 = planeOrigin - vec0*vecLen;
-				btVector3 pt2 = planeOrigin + vec1*vecLen;
-				btVector3 pt3 = planeOrigin - vec1*vecLen;
-				getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
-				getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+				getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
 				break;
 
 			}
diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h
index e7849f648..48c15806a 100644
--- a/src/LinearMath/btIDebugDraw.h
+++ b/src/LinearMath/btIDebugDraw.h
@@ -309,6 +309,107 @@ class	btIDebugDraw
 		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
 		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
 	}
+
+	virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
+	{
+		btVector3 capStart(0.f,0.f,0.f);
+		capStart[upAxis] = -halfHeight;
+
+		btVector3 capEnd(0.f,0.f,0.f);
+		capEnd[upAxis] = halfHeight;
+
+		// Draw the ends
+		{
+
+			btTransform childTransform = transform;
+			childTransform.getOrigin() = transform * capStart;
+			drawSphere(radius, childTransform, color);
+		}
+
+		{
+			btTransform childTransform = transform;
+			childTransform.getOrigin() = transform * capEnd;
+			drawSphere(radius, childTransform, color);
+		}
+
+		// 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);
+	}
+
+	virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
+	{
+		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);
+
+		// Drawing top and bottom caps of the cylinder
+		btVector3 yaxis(0,0,0);
+		yaxis[upAxis] = btScalar(1.0);
+		btVector3 xaxis(0,0,0);
+		xaxis[(upAxis+1)%3] = btScalar(1.0);
+		drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+		drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+	}
+
+	virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
+	{
+
+		btVector3 start = transform.getOrigin();
+
+		btVector3	offsetHeight(0,0,0);
+		offsetHeight[upAxis] = height * btScalar(0.5);
+		btVector3	offsetRadius(0,0,0);
+		offsetRadius[(upAxis+1)%3] = radius;
+		btVector3	offset2Radius(0,0,0);
+		offset2Radius[(upAxis+2)%3] = radius;
+
+		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);
+
+		// Drawing the base of the cone
+		btVector3 yaxis(0,0,0);
+		yaxis[upAxis] = btScalar(1.0);
+		btVector3 xaxis(0,0,0);
+		xaxis[(upAxis+1)%3] = btScalar(1.0);
+		drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
+	}
+
+	virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color)
+	{
+		btVector3 planeOrigin = planeNormal * planeConst;
+		btVector3 vec0,vec1;
+		btPlaneSpace1(planeNormal,vec0,vec1);
+		btScalar vecLen = 100.f;
+		btVector3 pt0 = planeOrigin + vec0*vecLen;
+		btVector3 pt1 = planeOrigin - vec0*vecLen;
+		btVector3 pt2 = planeOrigin + vec1*vecLen;
+		btVector3 pt3 = planeOrigin - vec1*vecLen;
+		drawLine(transform*pt0,transform*pt1,color);
+		drawLine(transform*pt2,transform*pt3,color);
+	}
 };