Add the Opportunity to debug the Bullet Physics visually

Core/Contents/Include/PolyEntity.h

@@ -256,6 +256,12 @@ namespace Polycode {
              * @see ownsChildren
              */
 			void setOwnsChildrenRecursive(bool val);
+
+			/**
+			* Removes all children
+			*/
+			void clearChildren();
+
 			//@}
 			// ----------------------------------------------------------------------------------------------------------------
 
@@ -889,6 +895,17 @@ namespace Polycode {
             void setLocalBoundingBoxZ(Number z);
 
             bool rendererVis;
+
+			/**
+			 * Sets forceRender to val. By default recursive.
+			 * @param val If true the Entity will always be rendered.
+			 */
+			void setForceRender(bool val, bool recursive = true);
+
+			/**
+			 * If set to true the Entity will always be rendered.
+			 */
+			bool forceRender;
 
             /**
             * Layer ID. Used by entity instances to separate entities into groups.

Core/Contents/Source/PolyEntity.cpp

@@ -78,6 +78,7 @@ void Entity::initEntity() {
 	yAdjust = 1.0;
 	lastClickTicks = 0.0;
     rendererVis = true;
+	forceRender = false;
     layerID = 0;
 }
 
@@ -157,6 +158,13 @@ void Entity::setOwnsChildrenRecursive(bool val) {
 	}
 }
 
+void Entity::clearChildren(){
+	for (int i = children.size()-1; i > 0; i--){
+		delete children[i];
+	}
+	children.clear();
+}
+
 std::vector<Entity*> Entity::getEntitiesByLayerID(unsigned char layerID, bool recursive) const {
 	std::vector<Entity*> retVector;
 
@@ -584,6 +592,15 @@ void Entity::renderChildren() {
 	}
 }
 
+void Entity::setForceRender(bool val, bool recursive){
+	forceRender = val;
+	if (recursive){
+		for (int i = 0; i<children.size(); i++) {
+			children[i]->setForceRender(val);
+		}
+	}
+}
+
 void Entity::dirtyMatrix(bool val) {
 	matrixDirty = val;
 }

Core/Contents/Source/PolyScene.cpp

@@ -204,7 +204,7 @@ void Scene::setEntityVisibilityBool(Entity *entity, bool val) {
 }
 
 void Scene::setEntityVisibility(Entity *entity, Camera *camera) {
-    if(camera->frustumCulling) {
+    if(camera->frustumCulling && !entity->forceRender) {
         entity->recalculateAABB();
         entity->rendererVis = camera->isAABBInFrustum(entity->getWorldAABB());
     } else {

Modules/Contents/3DPhysics/Include/PolyCollisionScene.h

@@ -33,6 +33,7 @@ class btCollisionWorld;
 namespace Polycode {
 
 class Entity;
+class SceneMesh;
 class CollisionEntity;
 
 /**
@@ -79,6 +80,29 @@ struct CollisionResult {
 		Vector3 position;
 	};
 
+	class CollisionSceneDebugDraw : public btIDebugDraw {
+	public:
+		CollisionSceneDebugDraw();
+
+		void Update(btCollisionWorld *world);
+		Entity* getDebugEntity();
+
+		void drawLine(const btVector3& from, const btVector3& to, const btVector3& color);
+		void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+		void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color){};
+		void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& color, btScalar alpha);
+
+		void reportErrorWarning(const char* warningString);
+		void draw3dText(const btVector3& location, const char* textString);
+		void setDebugMode(int debugMode);
+		int  getDebugMode() const;
+
+	protected:
+		Entity *debugEntity;
+		DebugDrawModes mode;
+		SceneMesh *lineMesh;
+	};
+
 	/**
 	* A scene that tracks collisions between entities. The collision scene acts like a regular scene, only it automatically tracks collisions between its child entities.
 	*/
@@ -93,6 +117,7 @@ struct CollisionResult {
 			void initCollisionScene(Vector3 size);
 
 			virtual void fixedUpdate();
+			void Update();
 
 			virtual void removeEntity(Entity *entity);
 
@@ -118,6 +143,13 @@ struct CollisionResult {
 			void removeCollision(Entity *entity);
 			void adjustForCollision(CollisionEntity *collisionEntity);
 
+			/**
+			 * Sets whether you are visually debugging the CollsionShapes
+			 * @param val If true you are debugging. Initially set to false.
+			 * @param mode Debug mode as int. Defaults to wireframe visualisation. For available values see "btIDebugDraw.h"
+			 */
+			void setDebug(bool val, int mode = 1);
+
 			//@}
 			// ----------------------------------------------------------------------------------------------------------------
 
@@ -129,6 +161,8 @@ struct CollisionResult {
 			btDefaultCollisionConfiguration *collisionConfiguration;
 			btCollisionDispatcher *dispatcher;
 			btAxisSweep3*  axisSweep;
+
+			CollisionSceneDebugDraw* debugDrawer;
 	};
 
 }

Modules/Contents/3DPhysics/Source/PolyCollisionScene.cpp

@@ -23,6 +23,9 @@ THE SOFTWARE.
 #include "PolyCollisionScene.h"
 #include "PolyCollisionSceneEntity.h"
 #include "PolyEntity.h"
+#include "PolySceneMesh.h"
+#include "PolySceneLabel.h"
+#include "PolyLogger.h"
 
 using namespace Polycode;
 
@@ -41,7 +44,10 @@ void CollisionScene::initCollisionScene(Vector3 size) {
 	dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	//	dispatcher->setNearCallback(customNearCallback);
 	axisSweep = new btAxisSweep3(worldAabbMin,worldAabbMax);
-	world = new btCollisionWorld(dispatcher,axisSweep,collisionConfiguration);	
+	world = new btCollisionWorld(dispatcher,axisSweep,collisionConfiguration);
+
+	debugDrawer = new CollisionSceneDebugDraw();
+	world->setDebugDrawer(debugDrawer);
 }
 
 void CollisionScene::fixedUpdate() {
@@ -60,6 +66,11 @@ void CollisionScene::fixedUpdate() {
 	Scene::fixedUpdate();
 }
 
+void CollisionScene::Update(){
+	debugDrawer->Update(world);
+	Scene::Update();
+}
+
 void CollisionScene::enableCollision(Entity *entity, bool val) {
 	CollisionEntity *cEnt = getCollisionByScreenEntity(entity);
 	if(cEnt) {
@@ -259,3 +270,130 @@ CollisionEntity *CollisionScene::addCollisionChild(Entity *newEntity, int type,
 	return trackCollision(newEntity, type, group);
 
 }
+
+void CollisionScene::setDebug(bool val, int mode){
+	if (val){
+		world->getDebugDrawer()->setDebugMode(mode);
+		this->addEntity(debugDrawer->getDebugEntity());
+	} else {
+		world->getDebugDrawer()->setDebugMode(0);
+		this->removeEntity(debugDrawer->getDebugEntity());
+	}
+}
+
+void CollisionSceneDebugDraw::drawLine(const btVector3& from, const btVector3& to, const btVector3& color){
+	lineMesh->getMesh()->addVertex(from.getX(), from.getY(), from.getZ());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getX());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getY());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getZ());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(1.0);
+	lineMesh->getMesh()->addVertex(to.getX(), to.getY(), to.getZ());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getX());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getY());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(color.getZ());
+	lineMesh->getMesh()->vertexColorArray.data.push_back(1.0);
+}
+
+void CollisionSceneDebugDraw::drawSphere(btScalar radius, const btTransform& transform, const btVector3& color){
+	ScenePrimitive* sphere = new ScenePrimitive(ScenePrimitive::TYPE_SPHERE, radius, 20, 20);
+	sphere->setPosition(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
+	sphere->setRotationQuat(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
+	sphere->setColor(color.getX(), color.getY(), color.getZ(), 1.0);
+	sphere->setMaterialByName("UnlitWireframe");
+
+	debugEntity->addChild(sphere);
+}
+
+void CollisionSceneDebugDraw::drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& color, btScalar alpha){
+	Mesh* mesh = new Mesh(Mesh::TRI_MESH);
+	mesh->addVertex(v0.getX(), v0.getY(), v0.getZ());
+	mesh->addVertex(v1.getX(), v1.getY(), v1.getZ());
+	mesh->addVertex(v2.getX(), v2.getY(), v2.getZ());
+
+	SceneMesh *sMesh = new SceneMesh(mesh);
+	sMesh->setColor(color.getX(), color.getY(), color.getZ(), 1.0);
+	sMesh->setMaterialByName("UnlitWireframe");
+
+	debugEntity->addChild(sMesh);
+}
+
+void CollisionSceneDebugDraw::reportErrorWarning(const char* warningString){
+	Logger::log(warningString);
+}
+
+void CollisionSceneDebugDraw::draw3dText(const btVector3& location, const char* textString){
+	SceneLabel* label = new SceneLabel(textString, 16);
+	label->setPosition(location.getX(), location.getY(), location.getZ());
+
+	debugEntity->addChild(label);
+}
+
+CollisionSceneDebugDraw::CollisionSceneDebugDraw(){
+	debugEntity = new Entity();
+
+	lineMesh = new SceneMesh(Mesh::LINE_MESH);
+	lineMesh->setMaterialByName("UnlitWireframe");
+	lineMesh->getMesh()->useVertexColors = true;
+	debugEntity->addChild(lineMesh);
+
+	mode = DBG_NoDebug;
+}
+
+void CollisionSceneDebugDraw::setDebugMode(int debugMode){
+	switch (debugMode) {
+	case 0:
+		mode = DBG_NoDebug;
+		break;
+	case 1:
+		mode = DBG_DrawWireframe;
+		break;
+	case 2:
+		mode = DBG_DrawAabb;
+		break;
+	case 4:
+		mode = DBG_DrawFeaturesText;
+		break;
+	case 8:
+		mode = DBG_DrawContactPoints;
+		break;
+	case 16:
+		mode = DBG_NoDeactivation;
+		break;
+	case 32:
+		mode = DBG_NoHelpText;
+		break;
+	case 64:
+		mode = DBG_DrawText;
+		break;
+	case 128:
+		mode = DBG_ProfileTimings;
+		break;
+	case 256:
+		mode = DBG_EnableSatComparison;
+		break;
+	case 512:
+		mode = DBG_DisableBulletLCP;
+		break;
+	case 1024:
+		mode = DBG_EnableCCD;
+		break;
+	default:
+		mode = DBG_NoDebug;
+		break;
+	}
+}
+int CollisionSceneDebugDraw::getDebugMode() const {
+	return mode;
+}
+
+void CollisionSceneDebugDraw::Update(btCollisionWorld* world){
+	debugEntity->clearChildren();
+	lineMesh->getMesh()->clearMesh();
+	debugEntity->addChild(lineMesh);
+	world->debugDrawWorld();
+	debugEntity->setForceRender(true, true);
+}
+
+Entity* CollisionSceneDebugDraw::getDebugEntity(){
+	return debugEntity;
+}

Modules/Contents/3DPhysics/Source/PolyPhysicsScene.cpp

@@ -94,6 +94,9 @@ void PhysicsScene::initPhysicsScene(Vector3 size) {
 	world = physicsWorld;
 
 	physicsWorld->setInternalTickCallback(worldTickCallback, this);
+
+	debugDrawer = new CollisionSceneDebugDraw();
+	physicsWorld->setDebugDrawer(debugDrawer);
 }
 
 void PhysicsScene::setGravity(Vector3 gravity) {