From 2bf17a7a819a51dc0d532bbe14c18fd03c35f7a2 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Fri, 1 Jul 2016 19:35:27 +0200
Subject: [PATCH 01/22] Initial commit.

------------
Simplifying the walker generation code and making it more understandable

SQUASH THIS LATER.
---
 examples/ExampleBrowser/CMakeLists.txt     |   2 +
 examples/ExampleBrowser/ExampleEntries.cpp |   3 +-
 examples/ExampleBrowser/premake4.lua       |   2 +
 examples/ExtendedTutorials/NN3DWalkers.cpp | 434 +++++++++++++++++++++
 examples/ExtendedTutorials/NN3DWalkers.h   |  23 ++
 5 files changed, 463 insertions(+), 1 deletion(-)
 create mode 100755 examples/ExtendedTutorials/NN3DWalkers.cpp
 create mode 100755 examples/ExtendedTutorials/NN3DWalkers.h

diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 0af74fdb8..49ccf0594 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -175,6 +175,8 @@ SET(BulletExampleBrowser_SRCS
 	../ExtendedTutorials/InclinedPlane.h
 	../ExtendedTutorials/MultiPendulum.cpp
 	../ExtendedTutorials/MultiPendulum.h
+	../ExtendedTutorials/NN3DWalkers.cpp
+	../ExtendedTutorials/NN3DWalkers.h
 	../Collision/CollisionSdkC_Api.cpp
 	../Collision/CollisionSdkC_Api.h
 	../Collision/CollisionTutorialBullet2.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 10cfe33ee..e32d14648 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -67,6 +67,7 @@
 #include "../ExtendedTutorials/InclinedPlane.h"
 #include "../ExtendedTutorials/NewtonsCradle.h"
 #include "../ExtendedTutorials/MultiPendulum.h"
+#include "../ExtendedTutorials/NN3DWalkers.h"
 
 struct ExampleEntry
 {
@@ -280,7 +281,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Inclined Plane", "Create an inclined plane to show restitution and different types of friction. Use the sliders to vary restitution and friction and press space to reset the scene.", ET_InclinedPlaneCreateFunc),
 	ExampleEntry(1,"Newton's Cradle", "Create a Newton's Cradle using a pair of point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.", ET_NewtonsCradleCreateFunc),
 	ExampleEntry(1,"Multi-Pendulum", "Create a Multi-Pendulum using point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.",ET_MultiPendulumCreateFunc),
-
+	ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",NN3DWalkersCreateFunc)
 
 	//todo: create a category/tutorial about advanced topics, such as optimizations, using different collision detection algorithm, different constraint solvers etc.
 	//ExampleEntry(0,"Advanced"),
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 82c63e64d..c872caa4c 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -93,6 +93,8 @@ project "App_BulletExampleBrowser"
 		"../ExtendedTutorials/Chain.cpp",
 		"../ExtendedTutorials/Bridge.cpp",
 		"../ExtendedTutorials/RigidBodyFromObj.cpp",
+		"../ExtendedTutorials/NN3DWalkers.cpp",
+		"../ExtendedTutorials/NN3DWalkers.h",
 		"../Collision/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
diff --git a/examples/ExtendedTutorials/NN3DWalkers.cpp b/examples/ExtendedTutorials/NN3DWalkers.cpp
new file mode 100755
index 000000000..c2a0648c7
--- /dev/null
+++ b/examples/ExtendedTutorials/NN3DWalkers.cpp
@@ -0,0 +1,434 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
+Motor Demo
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "NN3DWalkers.h"
+#include "btBulletDynamicsCommon.h"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btBroadphaseInterface;
+class btCollisionShape;
+class btOverlappingPairCache;
+class btCollisionDispatcher;
+class btConstraintSolver;
+struct btCollisionAlgorithmCreateFunc;
+class btDefaultCollisionConfiguration;
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+//TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
+
+class NN3DWalkers : public CommonRigidBodyBase
+{
+	float m_Time;
+	float m_fCyclePeriod; // in milliseconds
+	float m_fMuscleStrength;
+	
+	btAlignedObjectArray<class NNWalker*> m_walkers;
+	
+	
+public:
+	NN3DWalkers(struct GUIHelperInterface* helper)
+	:CommonRigidBodyBase(helper),m_fCyclePeriod(0),m_Time(0),m_fMuscleStrength(0)
+	{
+	}
+
+	void initPhysics();
+	
+	void exitPhysics();
+
+	virtual ~NN3DWalkers()
+	{
+	}
+	
+	void spawnWalker(const btVector3& startOffset, bool bFixed);
+	
+	virtual bool	keyboardCallback(int key, int state);
+	
+	void setMotorTargets(btScalar deltaTime);
+	
+	void resetCamera()
+	{
+		float dist = 11;
+		float pitch = 52;
+		float yaw = 35;
+		float targetPos[3]={0,0.46,0};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+
+	btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &point);
+	btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &axis);
+
+	btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &point);
+	btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &axis);
+};
+
+
+#ifndef M_PI
+#define M_PI       3.14159265358979323846
+#endif
+
+#ifndef M_PI_2
+#define M_PI_2     1.57079632679489661923
+#endif
+
+#ifndef M_PI_4
+#define M_PI_4     0.785398163397448309616
+#endif
+
+#ifndef M_PI_8
+#define M_PI_8     0.5 * M_PI_4
+#endif
+
+
+// /LOCAL FUNCTIONS
+
+
+
+#define NUM_LEGS 6
+#define BODYPART_COUNT 2 * NUM_LEGS + 1
+#define JOINT_COUNT BODYPART_COUNT - 1
+
+class NNWalker
+{
+	btDynamicsWorld*	m_ownerWorld;
+	btCollisionShape*	m_shapes[BODYPART_COUNT];
+	btRigidBody*		m_bodies[BODYPART_COUNT];
+	btTypedConstraint*	m_joints[JOINT_COUNT];
+
+	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
+	{
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0,0,0);
+		if (isDynamic)
+			shape->calculateLocalInertia(mass,localInertia);
+
+		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
+		btRigidBody* body = new btRigidBody(rbInfo);
+
+		m_ownerWorld->addRigidBody(body);
+
+		return body;
+	}
+
+
+public:
+	NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
+		: m_ownerWorld (ownerWorld)
+	{
+		btVector3 vUp(0, 1, 0);
+
+		//
+		// Setup geometry
+		//
+		float rootBodyRadius  = 0.25f;
+		float rootBodyHeight = 0.1f;
+		float legRadius = 0.1f;
+		float legLength = 0.45f;
+		float foreLegLength = 0.75f;
+		float foreLegRadius = 0.08f;
+		m_shapes[0] = new btCapsuleShape(btScalar(rootBodyRadius), btScalar(rootBodyHeight));
+		int i;
+		for ( i=0; i<NUM_LEGS; i++)
+		{
+			m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(legRadius), btScalar(legLength)); // leg  capsule
+			m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(foreLegRadius), btScalar(foreLegLength)); // fore leg capsule
+		}
+
+		//
+		// Setup rigid bodies
+		//
+		float footHeight = 0.5;
+		btTransform bodyOffset; bodyOffset.setIdentity();
+		bodyOffset.setOrigin(positionOffset);		
+
+		// root body
+		btVector3 localRootBodyPosition = btVector3(btScalar(0.), btScalar(footHeight), btScalar(0.)); // root body position in local reference frame
+		btTransform transform;
+		transform.setIdentity();
+		transform.setOrigin(localRootBodyPosition);
+		if (bFixed) // if fixed body
+		{
+			m_bodies[0] = localCreateRigidBody(btScalar(0.), bodyOffset*transform, m_shapes[0]);
+		} else
+		{
+			m_bodies[0] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[0]);
+		}
+
+		btHingeConstraint* hingeC;
+		//btConeTwistConstraint* coneC;
+
+		btTransform localA, localB, localC;
+
+		// legs
+		for ( i=0; i<NUM_LEGS; i++)
+		{
+			float footAngle = 2 * M_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
+			float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle
+			float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle
+
+			transform.setIdentity();
+			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(rootBodyRadius+0.5*legLength)), btScalar(footHeight), btScalar(footYUnitPosition*(rootBodyRadius+0.5*legLength)));
+			transform.setOrigin(legCOM);
+
+			// thigh
+			btVector3 legDirection = (legCOM - localRootBodyPosition).normalize();
+			btVector3 kneeAxis = legDirection.cross(vUp);			
+			transform.setRotation(btQuaternion(kneeAxis, M_PI_2));
+			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
+
+			// shin
+			transform.setIdentity();
+			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(footHeight-0.5*foreLegLength), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
+
+			//
+			// Setup the constraints
+			//
+
+			// hip joints
+			localA.setIdentity(); localB.setIdentity();
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*rootBodyRadius)));
+			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
+			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
+			hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
+			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
+			m_joints[2*i] = hingeC;
+			m_ownerWorld->addConstraint(m_joints[2*i], true);
+
+			// knee joints
+			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(0.), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
+			localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
+			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
+			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
+			hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
+			m_joints[1+2*i] = hingeC;
+			m_ownerWorld->addConstraint(m_joints[1+2*i], true);
+		}
+
+		// Setup some damping on the m_bodies
+		for (i = 0; i < BODYPART_COUNT; ++i)
+		{
+			m_bodies[i]->setDamping(0.05, 0.85);
+			m_bodies[i]->setDeactivationTime(0.8);
+			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
+			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
+		}
+	}
+
+	virtual	~NNWalker ()
+	{
+		int i;
+
+		// Remove all constraints
+		for ( i = 0; i < JOINT_COUNT; ++i)
+		{
+			m_ownerWorld->removeConstraint(m_joints[i]);
+			delete m_joints[i]; m_joints[i] = 0;
+		}
+
+		// Remove all bodies and shapes
+		for ( i = 0; i < BODYPART_COUNT; ++i)
+		{
+			m_ownerWorld->removeRigidBody(m_bodies[i]);
+			
+			delete m_bodies[i]->getMotionState();
+
+			delete m_bodies[i]; m_bodies[i] = 0;
+			delete m_shapes[i]; m_shapes[i] = 0;
+		}
+	}
+
+	btTypedConstraint** GetJoints() {return &m_joints[0];}
+
+};
+
+
+
+void motorNNPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
+{
+	NN3DWalkers* motorDemo = (NN3DWalkers*)world->getWorldUserInfo();
+
+	motorDemo->setMotorTargets(timeStep);
+	
+}
+
+
+
+void NN3DWalkers::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	// Setup the basic world
+
+	m_Time = 0;
+	m_fCyclePeriod = 2000.f; // in milliseconds
+
+//	m_fMuscleStrength = 0.05f;
+	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
+	// should be (numberOfsolverIterations * oldLimits)
+	// currently solver uses 10 iterations, so:
+	m_fMuscleStrength = 0.5f;
+
+	createEmptyDynamicsWorld();
+
+	m_dynamicsWorld->setInternalTickCallback(motorNNPreTickCallback,this,true);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	
+
+	// Setup a big ground box
+	{
+		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
+		m_collisionShapes.push_back(groundShape);
+		btTransform groundTransform;
+		groundTransform.setIdentity();
+		groundTransform.setOrigin(btVector3(0,-10,0));
+		createRigidBody(btScalar(0.),groundTransform,groundShape);
+	}
+
+	// Spawn one ragdoll
+	btVector3 startOffset(1,0.5,0);
+	spawnWalker(startOffset, false);
+	startOffset.setValue(-2,0.5,0);
+	spawnWalker(startOffset, true);
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+void NN3DWalkers::spawnWalker(const btVector3& startOffset, bool bFixed)
+{
+	NNWalker* walker = new NNWalker(m_dynamicsWorld, startOffset, bFixed);
+	m_walkers.push_back(walker);
+}
+
+//void	PreStep()
+//{
+//
+//}
+
+
+
+
+void NN3DWalkers::setMotorTargets(btScalar deltaTime)
+{
+
+	float ms = deltaTime*1000000.;
+	float minFPS = 1000000.f/60.f;
+	if (ms > minFPS)
+		ms = minFPS;
+
+	m_Time += ms;
+
+	//
+	// set per-frame sinusoidal position targets using angular motor (hacky?)
+	//	
+	for (int r=0; r<m_walkers.size(); r++)
+	{
+		for (int i=0; i<2*NUM_LEGS; i++)
+		{
+			btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->GetJoints()[i]);
+			btScalar fCurAngle      = hingeC->getHingeAngle();
+			
+			btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
+			btScalar fTargetAngle   = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
+			btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
+			btScalar fAngleError  = fTargetLimitAngle - fCurAngle;
+			btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
+			hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
+		}
+	}
+
+	
+}
+
+bool NN3DWalkers::keyboardCallback(int key, int state)
+{
+	switch (key)
+	{
+	case '+': case '=':
+		m_fCyclePeriod /= 1.1f;
+		if (m_fCyclePeriod < 1.f)
+			m_fCyclePeriod = 1.f;
+		return true;
+		break;
+	case '-': case '_':
+		m_fCyclePeriod *= 1.1f;
+		return true;
+		break;
+	case '[':
+		m_fMuscleStrength /= 1.1f;
+		return true;
+		break;
+	case ']':
+		m_fMuscleStrength *= 1.1f;
+		return true;
+		break;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+
+
+void NN3DWalkers::exitPhysics()
+{
+
+	int i;
+
+	for (i=0;i<m_walkers.size();i++)
+	{
+		NNWalker* walker = m_walkers[i];
+		delete walker;
+	}
+
+	CommonRigidBodyBase::exitPhysics();
+}
+
+
+class CommonExampleInterface*    NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
+{
+	return new NN3DWalkers(options.m_guiHelper);
+}
+
+
+btVector3 NN3DWalkers::getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 &point) {
+	  return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame
+}
+
+btVector3 NN3DWalkers::getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 &point) {
+	  return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame
+}
+
+btVector3 NN3DWalkers::getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &axis) {
+  btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame
+  btVector3 zero(0,0,0);
+  local1.setOrigin(zero);
+  return local1 * axis;
+}
+
+btVector3 NN3DWalkers::getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &axis) {
+  btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame
+  btVector3 zero(0,0,0);
+  local1.setOrigin(zero);
+  return local1 * axis;
+}
diff --git a/examples/ExtendedTutorials/NN3DWalkers.h b/examples/ExtendedTutorials/NN3DWalkers.h
new file mode 100755
index 000000000..7d7f22536
--- /dev/null
+++ b/examples/ExtendedTutorials/NN3DWalkers.h
@@ -0,0 +1,23 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
+Motor Demo
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef NN_3D_WALKERS_H
+#define NN_3D_WALKERS_H
+
+class CommonExampleInterface*    NN3DWalkersCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif

From e652863baa4dbd2d4dfb44e2c8dba604ead5ca18 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Sun, 3 Jul 2016 19:54:47 +0200
Subject: [PATCH 02/22] Working random walkers.

---
 examples/Evolution/premake4.lua            | 218 +++++++++++++++++++++
 examples/ExtendedTutorials/NN3DWalkers.cpp | 180 ++++++++++-------
 2 files changed, 327 insertions(+), 71 deletions(-)
 create mode 100755 examples/Evolution/premake4.lua

diff --git a/examples/Evolution/premake4.lua b/examples/Evolution/premake4.lua
new file mode 100755
index 000000000..ea1fc507e
--- /dev/null
+++ b/examples/Evolution/premake4.lua
@@ -0,0 +1,218 @@
+
+project "App_RigidBodyFromObjExample"
+
+if _OPTIONS["ios"] then
+	kind "WindowedApp"
+else	
+	kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+includedirs {"../../src"}
+
+links {
+	"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","BulletDynamics","BulletCollision", "LinearMath"
+}
+
+language "C++"
+
+files {
+	"RigidBodyFromObj.cpp",
+	"**.h",
+	"../StandaloneMain/main_console_single_example.cpp",
+		"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.h",
+			"../RenderingExamples/TimeSeriesCanvas.cpp",
+			"../RenderingExamples/TimeSeriesFontData.cpp",
+			"../MultiBody/InvertedPendulumPDControl.cpp",
+			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+			"../Importers/ImportURDFDemo/UrdfParser.cpp",
+			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+			
+}
+
+
+project "App_RigidBodyFromObjExampleGui"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+	initOpenGL()
+  initGlew()
+
+
+language "C++"
+
+files {
+        "RigidBodyFromObj.cpp",
+        "*.h",
+        "../StandaloneMain/main_opengl_single_example.cpp",
+	"../ExampleBrowser/OpenGLGuiHelper.cpp",
+	"../ExampleBrowser/GL_ShapeDrawer.cpp",
+	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+			"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.h",
+			"../RenderingExamples/TimeSeriesCanvas.cpp",
+			"../RenderingExamples/TimeSeriesFontData.cpp",
+			"../MultiBody/InvertedPendulumPDControl.cpp",
+			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+			"../Importers/ImportURDFDemo/UrdfParser.cpp",
+			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+        links{"Cocoa.framework"}
+end
+                          
+
+
+project "App_RigidBodyFromObjExampleGuiWithSoftwareRenderer"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+	initOpenGL()
+        initGlew()
+
+
+language "C++"
+
+files {
+        "RigidBodyFromObj.cpp",
+        "*.h",
+        "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp",
+	"../ExampleBrowser/OpenGLGuiHelper.cpp",
+	"../ExampleBrowser/GL_ShapeDrawer.cpp",
+	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../TinyRenderer/geometry.cpp",
+	"../TinyRenderer/model.cpp",
+	"../TinyRenderer/tgaimage.cpp",
+	"../TinyRenderer/our_gl.cpp",
+	"../TinyRenderer/TinyRenderer.cpp",
+	"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.h",
+			"../RenderingExamples/TimeSeriesCanvas.cpp",
+			"../RenderingExamples/TimeSeriesFontData.cpp",
+			"../MultiBody/InvertedPendulumPDControl.cpp",
+			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+			"../Importers/ImportURDFDemo/UrdfParser.cpp",
+			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+        links{"Cocoa.framework"}
+end
+                          
+
+
+project "App_RigidBodyFromObjExampleTinyRenderer"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "Bullet3Common"
+}
+
+
+language "C++"
+
+files {
+        "RigidBodyFromObj.cpp",
+        "*.h",
+        "../StandaloneMain/main_tinyrenderer_single_example.cpp",
+	"../OpenGLWindow/SimpleCamera.cpp",
+	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../TinyRenderer/geometry.cpp",
+	"../TinyRenderer/model.cpp",
+	"../TinyRenderer/tgaimage.cpp",
+	"../TinyRenderer/our_gl.cpp",
+	"../TinyRenderer/TinyRenderer.cpp",
+	"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.cpp",
+			"../Utils/b3ResourcePath.h",
+			"../RenderingExamples/TimeSeriesCanvas.cpp",
+			"../RenderingExamples/TimeSeriesFontData.cpp",
+			"../MultiBody/InvertedPendulumPDControl.cpp",
+			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+			"../Importers/ImportURDFDemo/UrdfParser.cpp",
+			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+
+}
+
diff --git a/examples/ExtendedTutorials/NN3DWalkers.cpp b/examples/ExtendedTutorials/NN3DWalkers.cpp
index c2a0648c7..4fc4dbf3a 100755
--- a/examples/ExtendedTutorials/NN3DWalkers.cpp
+++ b/examples/ExtendedTutorials/NN3DWalkers.cpp
@@ -31,9 +31,20 @@ class btDefaultCollisionConfiguration;
 
 //TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
 
+btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 point);
+btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis);
+
+btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 point);
+btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis);
+
+btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform);
+btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform);
+
 class NN3DWalkers : public CommonRigidBodyBase
 {
 	float m_Time;
+	float m_targetAccumulator;
+	float m_targetFrequency;
 	float m_fCyclePeriod; // in milliseconds
 	float m_fMuscleStrength;
 	
@@ -42,13 +53,13 @@ class NN3DWalkers : public CommonRigidBodyBase
 	
 public:
 	NN3DWalkers(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper),m_fCyclePeriod(0),m_Time(0),m_fMuscleStrength(0)
+	:CommonRigidBodyBase(helper),m_fCyclePeriod(0),m_Time(0),m_fMuscleStrength(0),m_targetFrequency(1),m_targetAccumulator(0)
 	{
-	}
 
+	}
 	void initPhysics();
 	
-	void exitPhysics();
+	virtual void exitPhysics();
 
 	virtual ~NN3DWalkers()
 	{
@@ -69,34 +80,21 @@ public:
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 	}
 
-	btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &point);
-	btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &axis);
-
-	btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &point);
-	btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &axis);
+	virtual void renderScene();
 };
 
+static NN3DWalkers* nn3DWalkers = NULL;
 
-#ifndef M_PI
-#define M_PI       3.14159265358979323846
+#ifndef SIMD_PI_4
+#define SIMD_PI_4     0.5 * SIMD_HALF_PI
 #endif
 
-#ifndef M_PI_2
-#define M_PI_2     1.57079632679489661923
+#ifndef SIMD_PI_8
+#define SIMD_PI_8     0.25 * SIMD_HALF_PI
 #endif
 
-#ifndef M_PI_4
-#define M_PI_4     0.785398163397448309616
-#endif
-
-#ifndef M_PI_8
-#define M_PI_8     0.5 * M_PI_4
-#endif
-
-
-// /LOCAL FUNCTIONS
-
-
+void* WALKER_ID = (void*)1;
+void* GROUND_ID = (void*)2;
 
 #define NUM_LEGS 6
 #define BODYPART_COUNT 2 * NUM_LEGS + 1
@@ -117,8 +115,8 @@ class NNWalker
 		if (isDynamic)
 			shape->calculateLocalInertia(mass,localInertia);
 
-		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
-		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
+		btDefaultMotionState* motionState = new btDefaultMotionState(startTransform);
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,shape,localInertia);
 		btRigidBody* body = new btRigidBody(rbInfo);
 
 		m_ownerWorld->addRigidBody(body);
@@ -131,7 +129,7 @@ public:
 	NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
 		: m_ownerWorld (ownerWorld)
 	{
-		btVector3 vUp(0, 1, 0);
+		btVector3 vUp(0, 1, 0); // up in local reference frame
 
 		//
 		// Setup geometry
@@ -178,7 +176,7 @@ public:
 		// legs
 		for ( i=0; i<NUM_LEGS; i++)
 		{
-			float footAngle = 2 * M_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
+			float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
 			float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle
 			float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle
 
@@ -189,7 +187,7 @@ public:
 			// thigh
 			btVector3 legDirection = (legCOM - localRootBodyPosition).normalize();
 			btVector3 kneeAxis = legDirection.cross(vUp);			
-			transform.setRotation(btQuaternion(kneeAxis, M_PI_2));
+			transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
 			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
 
 			// shin
@@ -204,9 +202,9 @@ public:
 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
 			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*rootBodyRadius)));
-			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
+			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
-			hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
+			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
 			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
 			m_joints[2*i] = hingeC;
 			m_ownerWorld->addConstraint(m_joints[2*i], true);
@@ -214,11 +212,11 @@ public:
 			// knee joints
 			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
 			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(0.), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
-			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
-			localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
+			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
+			localC = getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
 			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
-			hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
+			hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
 			m_joints[1+2*i] = hingeC;
 			m_ownerWorld->addConstraint(m_joints[1+2*i], true);
 		}
@@ -230,6 +228,7 @@ public:
 			m_bodies[i]->setDeactivationTime(0.8);
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
+			m_bodies[i]->setUserPointer(WALKER_ID);
 		}
 	}
 
@@ -262,7 +261,7 @@ public:
 
 
 
-void motorNNPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
+void legMotorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 {
 	NN3DWalkers* motorDemo = (NN3DWalkers*)world->getWorldUserInfo();
 
@@ -270,10 +269,34 @@ void motorNNPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 	
 }
 
+bool legContactProcessedCallback(btManifoldPoint& cp,
+                                void* body0, void* body1)
+{
+    void* ID1;
+	void* ID2;
+    btCollisionObject* o1 = static_cast<btCollisionObject*>(body0);
+    btCollisionObject* o2 = static_cast<btCollisionObject*>(body1);
+
+    ID1 = o1->getUserPointer();
+    ID2 = o2->getUserPointer();
+
+	if ((ID1 == GROUND_ID && ID2 == WALKER_ID) || (ID1 == WALKER_ID && ID2 == GROUND_ID)) {
+	    // Make a circle with a 0.9 radius at (0,0,0)
+	    // with RGB color (1,0,0).
+		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL)
+			nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
+	}
+    return false;
+}
+
 
 
 void NN3DWalkers::initPhysics()
 {
+	m_targetFrequency = 5;
+
+	gContactProcessedCallback = legContactProcessedCallback;
+
 	m_guiHelper->setUpAxis(1);
 
 	// Setup the basic world
@@ -289,7 +312,7 @@ void NN3DWalkers::initPhysics()
 
 	createEmptyDynamicsWorld();
 
-	m_dynamicsWorld->setInternalTickCallback(motorNNPreTickCallback,this,true);
+	m_dynamicsWorld->setInternalTickCallback(legMotorPreTickCallback,this,true);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	
 
@@ -300,14 +323,16 @@ void NN3DWalkers::initPhysics()
 		btTransform groundTransform;
 		groundTransform.setIdentity();
 		groundTransform.setOrigin(btVector3(0,-10,0));
-		createRigidBody(btScalar(0.),groundTransform,groundShape);
+		btRigidBody* ground = createRigidBody(btScalar(0.),groundTransform,groundShape);
+		ground->setFriction(5);
+		ground->setUserPointer(GROUND_ID);
 	}
 
-	// Spawn one ragdoll
-	btVector3 startOffset(1,0.5,0);
-	spawnWalker(startOffset, false);
-	startOffset.setValue(-2,0.5,0);
-	spawnWalker(startOffset, true);
+	for(int i = 0; i <20 ; i++){
+		// Spawn one walker
+		btVector3 startOffset(10*((double) rand() / (RAND_MAX)),0.5,10*((double) rand() / (RAND_MAX)));
+		spawnWalker(startOffset, false);
+	}
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
@@ -319,14 +344,6 @@ void NN3DWalkers::spawnWalker(const btVector3& startOffset, bool bFixed)
 	m_walkers.push_back(walker);
 }
 
-//void	PreStep()
-//{
-//
-//}
-
-
-
-
 void NN3DWalkers::setMotorTargets(btScalar deltaTime)
 {
 
@@ -337,26 +354,30 @@ void NN3DWalkers::setMotorTargets(btScalar deltaTime)
 
 	m_Time += ms;
 
-	//
-	// set per-frame sinusoidal position targets using angular motor (hacky?)
-	//	
-	for (int r=0; r<m_walkers.size(); r++)
+	m_targetAccumulator +=ms;
+
+	if(m_targetAccumulator >= 1000000.0f /((double)m_targetFrequency))
 	{
-		for (int i=0; i<2*NUM_LEGS; i++)
+		m_targetAccumulator = 0;
+		//
+		// set per-frame sinusoidal position targets using angular motor (hacky?)
+		//
+		for (int r=0; r<m_walkers.size(); r++)
 		{
-			btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->GetJoints()[i]);
-			btScalar fCurAngle      = hingeC->getHingeAngle();
-			
-			btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
-			btScalar fTargetAngle   = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
-			btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
-			btScalar fAngleError  = fTargetLimitAngle - fCurAngle;
-			btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
-			hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
+			for (int i=0; i<2*NUM_LEGS; i++)
+			{
+				btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->GetJoints()[i]);
+				btScalar fCurAngle      = hingeC->getHingeAngle();
+
+				btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
+				btScalar fTargetAngle   = ((double) rand() / (RAND_MAX));//0.5 * (1 + sin(2 * SIMD_PI * fTargetPercent+ i* SIMD_PI/NUM_LEGS));
+				btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
+				btScalar fAngleError  = fTargetLimitAngle - fCurAngle;
+				btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
+				hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
+			}
 		}
 	}
-
-	
 }
 
 bool NN3DWalkers::keyboardCallback(int key, int state)
@@ -404,31 +425,48 @@ void NN3DWalkers::exitPhysics()
 	CommonRigidBodyBase::exitPhysics();
 }
 
+void NN3DWalkers::renderScene()
+	{
+		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+
+		m_guiHelper->render(m_dynamicsWorld);
+
+		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
+	}
 
 class CommonExampleInterface*    NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
-	return new NN3DWalkers(options.m_guiHelper);
+	nn3DWalkers = new NN3DWalkers(options.m_guiHelper);
+	return nn3DWalkers;
 }
 
 
-btVector3 NN3DWalkers::getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 &point) {
+btVector3 getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 point) {
 	  return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame
 }
 
-btVector3 NN3DWalkers::getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 &point) {
+btVector3 getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 point) {
 	  return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame
 }
 
-btVector3 NN3DWalkers::getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &axis) {
+btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
   btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame
   btVector3 zero(0,0,0);
   local1.setOrigin(zero);
   return local1 * axis;
 }
 
-btVector3 NN3DWalkers::getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &axis) {
+btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
   btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame
   btVector3 zero(0,0,0);
   local1.setOrigin(zero);
   return local1 * axis;
 }
+
+btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+  return localObjectCenterOfMassTransform.inverse() * transform; // transforms the axis from the local frame into the world frame
+}
+
+btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+  return localObjectCenterOfMassTransform * transform; // transforms the axis from the local frame into the world frame
+}

From 277e103b7b3d68598014a737ae7a118e8cf5303c Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Mon, 4 Jul 2016 19:17:10 +0200
Subject: [PATCH 03/22] Move NN3DWalkers to the evolution folder.

---
 .../NN3DWalkers.cpp                           | 161 +++++++++----
 .../NN3DWalkers.h                             |   0
 examples/Evolution/premake4.lua               | 218 ------------------
 3 files changed, 110 insertions(+), 269 deletions(-)
 rename examples/{ExtendedTutorials => Evolution}/NN3DWalkers.cpp (78%)
 rename examples/{ExtendedTutorials => Evolution}/NN3DWalkers.h (100%)
 delete mode 100755 examples/Evolution/premake4.lua

diff --git a/examples/ExtendedTutorials/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
similarity index 78%
rename from examples/ExtendedTutorials/NN3DWalkers.cpp
rename to examples/Evolution/NN3DWalkers.cpp
index 4fc4dbf3a..a74eb3c9b 100755
--- a/examples/ExtendedTutorials/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -15,6 +15,8 @@ subject to the following restrictions:
 
 
 #include "NN3DWalkers.h"
+#include <map>
+
 #include "btBulletDynamicsCommon.h"
 
 #include "LinearMath/btIDebugDraw.h"
@@ -28,6 +30,7 @@ struct btCollisionAlgorithmCreateFunc;
 class btDefaultCollisionConfiguration;
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
 
 //TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
 
@@ -42,18 +45,17 @@ btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransfor
 
 class NN3DWalkers : public CommonRigidBodyBase
 {
-	float m_Time;
-	float m_targetAccumulator;
-	float m_targetFrequency;
-	float m_fCyclePeriod; // in milliseconds
-	float m_fMuscleStrength;
+	btScalar m_Time;
+	btScalar m_targetAccumulator;
+	btScalar m_targetFrequency;
+	btScalar m_motorStrength;
 	
 	btAlignedObjectArray<class NNWalker*> m_walkers;
 	
 	
 public:
 	NN3DWalkers(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper),m_fCyclePeriod(0),m_Time(0),m_fMuscleStrength(0),m_targetFrequency(1),m_targetAccumulator(0)
+	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0)
 	{
 
 	}
@@ -93,12 +95,12 @@ static NN3DWalkers* nn3DWalkers = NULL;
 #define SIMD_PI_8     0.25 * SIMD_HALF_PI
 #endif
 
-void* WALKER_ID = (void*)1;
-void* GROUND_ID = (void*)2;
+void* GROUND_ID = (void*)1;
+bool RANDOM_MOVEMENT = false;
 
 #define NUM_LEGS 6
-#define BODYPART_COUNT 2 * NUM_LEGS + 1
-#define JOINT_COUNT BODYPART_COUNT - 1
+#define BODYPART_COUNT (2 * NUM_LEGS + 1)
+#define JOINT_COUNT (BODYPART_COUNT - 1)
 
 class NNWalker
 {
@@ -106,6 +108,9 @@ class NNWalker
 	btCollisionShape*	m_shapes[BODYPART_COUNT];
 	btRigidBody*		m_bodies[BODYPART_COUNT];
 	btTypedConstraint*	m_joints[JOINT_COUNT];
+	std::map<void*,int>			m_body_index_map;
+	bool 				m_touch_sensors[BODYPART_COUNT];
+	float 				m_sensory_motor_weights[BODYPART_COUNT*JOINT_COUNT];
 
 	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
 	{
@@ -131,6 +136,13 @@ public:
 	{
 		btVector3 vUp(0, 1, 0); // up in local reference frame
 
+		//initialize random weights
+		for(int i = 0;i < BODYPART_COUNT;i++){
+			for(int j = 0;j < JOINT_COUNT;j++){
+				m_sensory_motor_weights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
+			}
+		}
+
 		//
 		// Setup geometry
 		//
@@ -228,7 +240,9 @@ public:
 			m_bodies[i]->setDeactivationTime(0.8);
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
-			m_bodies[i]->setUserPointer(WALKER_ID);
+			m_bodies[i]->setUserPointer(this);
+			m_body_index_map.insert(std::pair<void*,int>(m_bodies[i],i));
+
 		}
 	}
 
@@ -255,8 +269,23 @@ public:
 		}
 	}
 
-	btTypedConstraint** GetJoints() {return &m_joints[0];}
+	btTypedConstraint** getJoints() {return &m_joints[0];}
 
+	void setTouchSensor(void* bodyPointer){
+		m_touch_sensors[m_body_index_map.at(bodyPointer)] = true;
+	}
+
+	void clearTouchSensors(){
+		for(int i = 0 ; i < BODYPART_COUNT;i++){
+			m_touch_sensors[i] = false;
+		}
+	}
+
+	bool getTouchSensor(int i){ return m_touch_sensors[i];}
+
+	const float* getSensoryMotorWeights() const {
+		return m_sensory_motor_weights;
+	}
 };
 
 
@@ -272,19 +301,25 @@ void legMotorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 bool legContactProcessedCallback(btManifoldPoint& cp,
                                 void* body0, void* body1)
 {
-    void* ID1;
-	void* ID2;
     btCollisionObject* o1 = static_cast<btCollisionObject*>(body0);
     btCollisionObject* o2 = static_cast<btCollisionObject*>(body1);
 
-    ID1 = o1->getUserPointer();
-    ID2 = o2->getUserPointer();
+    void* ID1 = o1->getUserPointer();
+    void* ID2 = o2->getUserPointer();
 
-	if ((ID1 == GROUND_ID && ID2 == WALKER_ID) || (ID1 == WALKER_ID && ID2 == GROUND_ID)) {
+	if (ID2 != GROUND_ID || ID1 != GROUND_ID) {
 	    // Make a circle with a 0.9 radius at (0,0,0)
 	    // with RGB color (1,0,0).
 		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL)
 			nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
+
+		if(ID1 != GROUND_ID){
+			((NNWalker*)ID1)->setTouchSensor(o1);
+		}
+
+		if(ID2 != GROUND_ID){
+			((NNWalker*)ID2)->setTouchSensor(o2);
+		}
 	}
     return false;
 }
@@ -293,8 +328,6 @@ bool legContactProcessedCallback(btManifoldPoint& cp,
 
 void NN3DWalkers::initPhysics()
 {
-	m_targetFrequency = 5;
-
 	gContactProcessedCallback = legContactProcessedCallback;
 
 	m_guiHelper->setUpAxis(1);
@@ -302,18 +335,36 @@ void NN3DWalkers::initPhysics()
 	// Setup the basic world
 
 	m_Time = 0;
-	m_fCyclePeriod = 2000.f; // in milliseconds
-
-//	m_fMuscleStrength = 0.05f;
-	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
-	// should be (numberOfsolverIterations * oldLimits)
-	// currently solver uses 10 iterations, so:
-	m_fMuscleStrength = 0.5f;
 
 	createEmptyDynamicsWorld();
 
 	m_dynamicsWorld->setInternalTickCallback(legMotorPreTickCallback,this,true);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	m_targetFrequency = 3;
+
+	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
+	// should be (numberOfsolverIterations * oldLimits)
+	m_motorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations;
+
+
+	{ // create a slider to change the motor update frequency
+		SliderParams slider("Motor update frequency", &m_targetFrequency);
+		slider.m_minVal = 0;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
+	{ // create a slider to change the motor torque
+		SliderParams slider("Motor force", &m_motorStrength);
+		slider.m_minVal = 1;
+		slider.m_maxVal = 50;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
 	
 
 	// Setup a big ground box
@@ -328,10 +379,13 @@ void NN3DWalkers::initPhysics()
 		ground->setUserPointer(GROUND_ID);
 	}
 
-	for(int i = 0; i <20 ; i++){
-		// Spawn one walker
-		btVector3 startOffset(10*((double) rand() / (RAND_MAX)),0.5,10*((double) rand() / (RAND_MAX)));
-		spawnWalker(startOffset, false);
+	for(int i = 0; i < 5 ; i++){
+		for(int j = 0; j < 5; j++){
+			// Spawn one walker
+			btVector3 spacing(10.0f,0.8f,10.0f);
+			btVector3 startOffset(spacing * btVector3(i,0,j));
+			spawnWalker(startOffset, false);
+		}
 	}
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@@ -366,16 +420,31 @@ void NN3DWalkers::setMotorTargets(btScalar deltaTime)
 		{
 			for (int i=0; i<2*NUM_LEGS; i++)
 			{
-				btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->GetJoints()[i]);
-				btScalar fCurAngle      = hingeC->getHingeAngle();
+				btScalar targetAngle = 0;
+				btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->getJoints()[i]);
 
-				btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
-				btScalar fTargetAngle   = ((double) rand() / (RAND_MAX));//0.5 * (1 + sin(2 * SIMD_PI * fTargetPercent+ i* SIMD_PI/NUM_LEGS));
-				btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
-				btScalar fAngleError  = fTargetLimitAngle - fCurAngle;
-				btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
-				hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
+				if(RANDOM_MOVEMENT){
+					targetAngle   = ((double) rand() / (RAND_MAX));//0.5 * (1 + sin(2 * SIMD_PI * fTargetPercent+ i* SIMD_PI/NUM_LEGS));
+				}
+				else{
+
+					// accumulate sensor inputs with weights
+					for(int j = 0; j < JOINT_COUNT;j++){
+						targetAngle += m_walkers[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkers[r]->getTouchSensor(i);
+					}
+
+					// apply the activation function
+					targetAngle = (tanh(targetAngle)+1.0f)*0.5f;
+				}
+				btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
+				btScalar currentAngle      = hingeC->getHingeAngle();
+				btScalar angleError  = targetLimitAngle - currentAngle;
+				btScalar desiredAngularVel = 1000000.f * angleError/ms;
+				hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength);
 			}
+
+			// clear sensor signals after usage
+			m_walkers[r]->clearTouchSensors();
 		}
 	}
 }
@@ -384,22 +453,12 @@ bool NN3DWalkers::keyboardCallback(int key, int state)
 {
 	switch (key)
 	{
-	case '+': case '=':
-		m_fCyclePeriod /= 1.1f;
-		if (m_fCyclePeriod < 1.f)
-			m_fCyclePeriod = 1.f;
-		return true;
-		break;
-	case '-': case '_':
-		m_fCyclePeriod *= 1.1f;
-		return true;
-		break;
 	case '[':
-		m_fMuscleStrength /= 1.1f;
+		m_motorStrength /= 1.1f;
 		return true;
 		break;
 	case ']':
-		m_fMuscleStrength *= 1.1f;
+		m_motorStrength *= 1.1f;
 		return true;
 		break;
 	default:
@@ -434,7 +493,7 @@ void NN3DWalkers::renderScene()
 		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
 	}
 
-class CommonExampleInterface*    NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
+class CommonExampleInterface*    ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
 	nn3DWalkers = new NN3DWalkers(options.m_guiHelper);
 	return nn3DWalkers;
diff --git a/examples/ExtendedTutorials/NN3DWalkers.h b/examples/Evolution/NN3DWalkers.h
similarity index 100%
rename from examples/ExtendedTutorials/NN3DWalkers.h
rename to examples/Evolution/NN3DWalkers.h
diff --git a/examples/Evolution/premake4.lua b/examples/Evolution/premake4.lua
deleted file mode 100755
index ea1fc507e..000000000
--- a/examples/Evolution/premake4.lua
+++ /dev/null
@@ -1,218 +0,0 @@
-
-project "App_RigidBodyFromObjExample"
-
-if _OPTIONS["ios"] then
-	kind "WindowedApp"
-else	
-	kind "ConsoleApp"
-end
-defines {"B3_USE_STANDALONE_EXAMPLE"}
-includedirs {"../../src"}
-
-links {
-	"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","BulletDynamics","BulletCollision", "LinearMath"
-}
-
-language "C++"
-
-files {
-	"RigidBodyFromObj.cpp",
-	"**.h",
-	"../StandaloneMain/main_console_single_example.cpp",
-		"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.h",
-			"../RenderingExamples/TimeSeriesCanvas.cpp",
-			"../RenderingExamples/TimeSeriesFontData.cpp",
-			"../MultiBody/InvertedPendulumPDControl.cpp",
-			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
-			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
-			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
-			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
-			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
-			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
-			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
-			"../Importers/ImportURDFDemo/UrdfParser.cpp",
-			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-			
-}
-
-
-project "App_RigidBodyFromObjExampleGui"
-
-if _OPTIONS["ios"] then
-        kind "WindowedApp"
-else
-        kind "ConsoleApp"
-end
-defines {"B3_USE_STANDALONE_EXAMPLE"}
-
-includedirs {"../../src"}
-
-links {
-        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
-}
-
-	initOpenGL()
-  initGlew()
-
-
-language "C++"
-
-files {
-        "RigidBodyFromObj.cpp",
-        "*.h",
-        "../StandaloneMain/main_opengl_single_example.cpp",
-	"../ExampleBrowser/OpenGLGuiHelper.cpp",
-	"../ExampleBrowser/GL_ShapeDrawer.cpp",
-	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
-			"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.h",
-			"../RenderingExamples/TimeSeriesCanvas.cpp",
-			"../RenderingExamples/TimeSeriesFontData.cpp",
-			"../MultiBody/InvertedPendulumPDControl.cpp",
-			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
-			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
-			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
-			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
-			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
-			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
-			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
-			"../Importers/ImportURDFDemo/UrdfParser.cpp",
-			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
-}
-
-if os.is("Linux") then initX11() end
-
-if os.is("MacOSX") then
-        links{"Cocoa.framework"}
-end
-                          
-
-
-project "App_RigidBodyFromObjExampleGuiWithSoftwareRenderer"
-
-if _OPTIONS["ios"] then
-        kind "WindowedApp"
-else
-        kind "ConsoleApp"
-end
-defines {"B3_USE_STANDALONE_EXAMPLE"}
-
-includedirs {"../../src"}
-
-links {
-        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
-}
-
-	initOpenGL()
-        initGlew()
-
-
-language "C++"
-
-files {
-        "RigidBodyFromObj.cpp",
-        "*.h",
-        "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp",
-	"../ExampleBrowser/OpenGLGuiHelper.cpp",
-	"../ExampleBrowser/GL_ShapeDrawer.cpp",
-	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
-	"../TinyRenderer/geometry.cpp",
-	"../TinyRenderer/model.cpp",
-	"../TinyRenderer/tgaimage.cpp",
-	"../TinyRenderer/our_gl.cpp",
-	"../TinyRenderer/TinyRenderer.cpp",
-	"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.h",
-			"../RenderingExamples/TimeSeriesCanvas.cpp",
-			"../RenderingExamples/TimeSeriesFontData.cpp",
-			"../MultiBody/InvertedPendulumPDControl.cpp",
-			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
-			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
-			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
-			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
-			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
-			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
-			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
-			"../Importers/ImportURDFDemo/UrdfParser.cpp",
-			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
-}
-
-if os.is("Linux") then initX11() end
-
-if os.is("MacOSX") then
-        links{"Cocoa.framework"}
-end
-                          
-
-
-project "App_RigidBodyFromObjExampleTinyRenderer"
-
-if _OPTIONS["ios"] then
-        kind "WindowedApp"
-else
-        kind "ConsoleApp"
-end
-defines {"B3_USE_STANDALONE_EXAMPLE"}
-
-includedirs {"../../src"}
-
-links {
-        "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "Bullet3Common"
-}
-
-
-language "C++"
-
-files {
-        "RigidBodyFromObj.cpp",
-        "*.h",
-        "../StandaloneMain/main_tinyrenderer_single_example.cpp",
-	"../OpenGLWindow/SimpleCamera.cpp",
-	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
-	"../TinyRenderer/geometry.cpp",
-	"../TinyRenderer/model.cpp",
-	"../TinyRenderer/tgaimage.cpp",
-	"../TinyRenderer/our_gl.cpp",
-	"../TinyRenderer/TinyRenderer.cpp",
-	"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.cpp",
-			"../Utils/b3ResourcePath.h",
-			"../RenderingExamples/TimeSeriesCanvas.cpp",
-			"../RenderingExamples/TimeSeriesFontData.cpp",
-			"../MultiBody/InvertedPendulumPDControl.cpp",
-			"../ThirdPartyLibs/tinyxml/tinystr.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
-			"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
-			"../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
-			"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
-			"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
-			"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
-			"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
-			"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
-			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
-			"../Importers/ImportURDFDemo/UrdfParser.cpp",
-			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
-}
-

From 74aba8b8fd6460fdf8aa68576b112311c1a4c1b2 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Mon, 4 Jul 2016 19:17:50 +0200
Subject: [PATCH 04/22] Correct the build files and fix the name of the create
 method in header file.

---
 examples/Evolution/NN3DWalkers.h           |  2 +-
 examples/ExampleBrowser/CMakeLists.txt     | 28 +++++++++++-----------
 examples/ExampleBrowser/ExampleEntries.cpp |  6 +++--
 examples/ExampleBrowser/premake4.lua       |  4 ++--
 4 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.h b/examples/Evolution/NN3DWalkers.h
index 7d7f22536..4f2f4b769 100755
--- a/examples/Evolution/NN3DWalkers.h
+++ b/examples/Evolution/NN3DWalkers.h
@@ -17,7 +17,7 @@ subject to the following restrictions:
 #ifndef NN_3D_WALKERS_H
 #define NN_3D_WALKERS_H
 
-class CommonExampleInterface*    NN3DWalkersCreateFunc(struct CommonExampleOptions& options);
+class CommonExampleInterface*    ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options);
 
 
 #endif
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 49ccf0594..0b698742b 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -107,18 +107,18 @@ ELSE(WIN32)
 ENDIF(WIN32)
 
 
-SET(ExtendedTutorialsSources
-	../ExtendedTutorials/SimpleBox.cpp
-	../ExtendedTutorials/MultipleBoxes.cpp
-	../ExtendedTutorials/SimpleJoint.cpp
-	../ExtendedTutorials/SimpleCloth.cpp
-	../ExtendedTutorials/Chain.cpp 
-	../ExtendedTutorials/Bridge.cpp 
-	../ExtendedTutorials/RigidBodyFromObj.cpp
-	../ExtendedTutorials/InclinedPlane.cpp
-	../ExtendedTutorials/InclinedPlane.h
-	../ExtendedTutorials/NewtonsCradle.cpp
-)
+#SET(ExtendedTutorialsSources
+#	../ExtendedTutorials/SimpleBox.cpp
+#	../ExtendedTutorials/MultipleBoxes.cpp
+#	../ExtendedTutorials/SimpleJoint.cpp
+#	../ExtendedTutorials/SimpleCloth.cpp
+#	../ExtendedTutorials/Chain.cpp 
+#	../ExtendedTutorials/Bridge.cpp 
+#	../ExtendedTutorials/RigidBodyFromObj.cpp
+#	../ExtendedTutorials/InclinedPlane.cpp
+#	../ExtendedTutorials/InclinedPlane.h
+#	../ExtendedTutorials/NewtonsCradle.cpp
+#)
 
 SET(BulletExampleBrowser_SRCS
 	
@@ -175,8 +175,8 @@ SET(BulletExampleBrowser_SRCS
 	../ExtendedTutorials/InclinedPlane.h
 	../ExtendedTutorials/MultiPendulum.cpp
 	../ExtendedTutorials/MultiPendulum.h
-	../ExtendedTutorials/NN3DWalkers.cpp
-	../ExtendedTutorials/NN3DWalkers.h
+	../Evolution/NN3DWalkers.cpp
+	../Evolution/NN3DWalkers.h
 	../Collision/CollisionSdkC_Api.cpp
 	../Collision/CollisionSdkC_Api.h
 	../Collision/CollisionTutorialBullet2.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index e32d14648..fed2d4690 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -67,7 +67,7 @@
 #include "../ExtendedTutorials/InclinedPlane.h"
 #include "../ExtendedTutorials/NewtonsCradle.h"
 #include "../ExtendedTutorials/MultiPendulum.h"
-#include "../ExtendedTutorials/NN3DWalkers.h"
+#include "../Evolution/NN3DWalkers.h"
 
 struct ExampleEntry
 {
@@ -281,7 +281,9 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Inclined Plane", "Create an inclined plane to show restitution and different types of friction. Use the sliders to vary restitution and friction and press space to reset the scene.", ET_InclinedPlaneCreateFunc),
 	ExampleEntry(1,"Newton's Cradle", "Create a Newton's Cradle using a pair of point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.", ET_NewtonsCradleCreateFunc),
 	ExampleEntry(1,"Multi-Pendulum", "Create a Multi-Pendulum using point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.",ET_MultiPendulumCreateFunc),
-	ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",NN3DWalkersCreateFunc)
+
+	ExampleEntry(9,"Evolution"),
+	ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",ET_NN3DWalkersCreateFunc),
 
 	//todo: create a category/tutorial about advanced topics, such as optimizations, using different collision detection algorithm, different constraint solvers etc.
 	//ExampleEntry(0,"Advanced"),
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index c872caa4c..c5299244a 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -93,8 +93,8 @@ project "App_BulletExampleBrowser"
 		"../ExtendedTutorials/Chain.cpp",
 		"../ExtendedTutorials/Bridge.cpp",
 		"../ExtendedTutorials/RigidBodyFromObj.cpp",
-		"../ExtendedTutorials/NN3DWalkers.cpp",
-		"../ExtendedTutorials/NN3DWalkers.h",
+		"../Evolution/NN3DWalkers.cpp",
+		"../Evolution/NN3DWalkers.h",
 		"../Collision/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",

From 40175b4700a73eb94636b7d752e2a923279555b9 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Mon, 4 Jul 2016 19:26:27 +0200
Subject: [PATCH 05/22] Changed header guard name.

---
 examples/Evolution/NN3DWalkers.h | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.h b/examples/Evolution/NN3DWalkers.h
index 4f2f4b769..0e6f0c7e6 100755
--- a/examples/Evolution/NN3DWalkers.h
+++ b/examples/Evolution/NN3DWalkers.h
@@ -13,11 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-#ifndef NN_3D_WALKERS_H
-#define NN_3D_WALKERS_H
+#ifndef ET_NN_3D_WALKERS_EXAMPLE_H
+#define ET_NN_3D_WALKERS_EXAMPLE_H
 
 class CommonExampleInterface*    ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options);
 
 
-#endif
+#endif //ET_NN_3D_WALKERS_EXAMPLE_H

From 34ae030c72f2b0768c2f868f86357c0c6f64206e Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Mon, 4 Jul 2016 19:27:18 +0200
Subject: [PATCH 06/22] Fix CMake references.

---
 examples/ExampleBrowser/CMakeLists.txt | 48 ++++++++++++--------------
 1 file changed, 22 insertions(+), 26 deletions(-)

diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 0b698742b..e6570b890 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -107,18 +107,28 @@ ELSE(WIN32)
 ENDIF(WIN32)
 
 
-#SET(ExtendedTutorialsSources
-#	../ExtendedTutorials/SimpleBox.cpp
-#	../ExtendedTutorials/MultipleBoxes.cpp
-#	../ExtendedTutorials/SimpleJoint.cpp
-#	../ExtendedTutorials/SimpleCloth.cpp
-#	../ExtendedTutorials/Chain.cpp 
-#	../ExtendedTutorials/Bridge.cpp 
-#	../ExtendedTutorials/RigidBodyFromObj.cpp
-#	../ExtendedTutorials/InclinedPlane.cpp
-#	../ExtendedTutorials/InclinedPlane.h
-#	../ExtendedTutorials/NewtonsCradle.cpp
-#)
+SET(ExtendedTutorialsSources
+	../ExtendedTutorials/Chain.cpp 
+	../ExtendedTutorials/Chain.h
+	../ExtendedTutorials/Bridge.cpp
+	../ExtendedTutorials/Bridge.h
+	../ExtendedTutorials/RigidBodyFromObj.cpp
+	../ExtendedTutorials/RigidBodyFromObj.h
+    ../ExtendedTutorials/SimpleBox.cpp
+	../ExtendedTutorials/SimpleBox.h
+	../ExtendedTutorials/MultipleBoxes.cpp
+	../ExtendedTutorials/MultipleBoxes.h
+	../ExtendedTutorials/SimpleCloth.cpp
+	../ExtendedTutorials/SimpleCloth.h
+	../ExtendedTutorials/SimpleJoint.cpp
+	../ExtendedTutorials/SimpleJoint.h
+	../ExtendedTutorials/NewtonsCradle.cpp
+	../ExtendedTutorials/NewtonsCradle.h
+	../ExtendedTutorials/InclinedPlane.cpp
+	../ExtendedTutorials/InclinedPlane.h
+	../ExtendedTutorials/MultiPendulum.cpp
+	../ExtendedTutorials/MultiPendulum.h
+)
 
 SET(BulletExampleBrowser_SRCS
 	
@@ -161,20 +171,6 @@ SET(BulletExampleBrowser_SRCS
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
 	../Tutorial/Dof6ConstraintTutorial.h
-	../ExtendedTutorials/SimpleBox.cpp
-	../ExtendedTutorials/SimpleBox.h
-	../ExtendedTutorials/MultipleBoxes.cpp
-	../ExtendedTutorials/MultipleBoxes.h
-	../ExtendedTutorials/SimpleCloth.cpp
-	../ExtendedTutorials/SimpleCloth.h
-	../ExtendedTutorials/SimpleJoint.cpp
-	../ExtendedTutorials/SimpleJoint.h
-	../ExtendedTutorials/NewtonsCradle.cpp
-	../ExtendedTutorials/NewtonsCradle.h
-	../ExtendedTutorials/InclinedPlane.cpp
-	../ExtendedTutorials/InclinedPlane.h
-	../ExtendedTutorials/MultiPendulum.cpp
-	../ExtendedTutorials/MultiPendulum.h
 	../Evolution/NN3DWalkers.cpp
 	../Evolution/NN3DWalkers.h
 	../Collision/CollisionSdkC_Api.cpp

From a71810e76f341c360811a6d052411f0b04587587 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Mon, 4 Jul 2016 19:43:26 +0200
Subject: [PATCH 07/22] Change scalings of different body parts.

---
 examples/Evolution/NN3DWalkers.cpp | 107 ++++++++++++++++++++++-------
 1 file changed, 81 insertions(+), 26 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index a74eb3c9b..d6f993bbc 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -35,14 +35,21 @@ class btDefaultCollisionConfiguration;
 //TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
 
 btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 point);
-btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis);
-
 btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 point);
+
 btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis);
+btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis);
 
 btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform);
 btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform);
 
+static btScalar gRootBodyRadius  = 0.25f;
+static btScalar gRootBodyHeight = 0.1f;
+static btScalar gLegRadius = 0.1f;
+static btScalar gLegLength = 0.45f;
+static btScalar gForeLegLength = 0.75f;
+static btScalar gForeLegRadius = 0.08f;
+
 class NN3DWalkers : public CommonRigidBodyBase
 {
 	btScalar m_Time;
@@ -108,9 +115,9 @@ class NNWalker
 	btCollisionShape*	m_shapes[BODYPART_COUNT];
 	btRigidBody*		m_bodies[BODYPART_COUNT];
 	btTypedConstraint*	m_joints[JOINT_COUNT];
-	std::map<void*,int>			m_body_index_map;
-	bool 				m_touch_sensors[BODYPART_COUNT];
-	float 				m_sensory_motor_weights[BODYPART_COUNT*JOINT_COUNT];
+	std::map<void*,int>			m_bodyIndexMap;
+	bool 				m_touchSensors[BODYPART_COUNT];
+	float 				m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
 
 	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
 	{
@@ -139,36 +146,30 @@ public:
 		//initialize random weights
 		for(int i = 0;i < BODYPART_COUNT;i++){
 			for(int j = 0;j < JOINT_COUNT;j++){
-				m_sensory_motor_weights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
+				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
 			}
 		}
 
 		//
 		// Setup geometry
 		//
-		float rootBodyRadius  = 0.25f;
-		float rootBodyHeight = 0.1f;
-		float legRadius = 0.1f;
-		float legLength = 0.45f;
-		float foreLegLength = 0.75f;
-		float foreLegRadius = 0.08f;
-		m_shapes[0] = new btCapsuleShape(btScalar(rootBodyRadius), btScalar(rootBodyHeight));
+		m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule
 		int i;
 		for ( i=0; i<NUM_LEGS; i++)
 		{
-			m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(legRadius), btScalar(legLength)); // leg  capsule
-			m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(foreLegRadius), btScalar(foreLegLength)); // fore leg capsule
+			m_shapes[1 + 2*i] = new btCapsuleShape(gLegRadius, gLegLength); // leg  capsule
+			m_shapes[2 + 2*i] = new btCapsuleShape(gForeLegRadius, gForeLegLength); // fore leg capsule
 		}
 
 		//
 		// Setup rigid bodies
 		//
-		float footHeight = 0.5;
+		float rootAboveGroundHeight = gForeLegLength;
 		btTransform bodyOffset; bodyOffset.setIdentity();
 		bodyOffset.setOrigin(positionOffset);		
 
 		// root body
-		btVector3 localRootBodyPosition = btVector3(btScalar(0.), btScalar(footHeight), btScalar(0.)); // root body position in local reference frame
+		btVector3 localRootBodyPosition = btVector3(btScalar(0.), btScalar(rootAboveGroundHeight), btScalar(0.)); // root body position in local reference frame
 		btTransform transform;
 		transform.setIdentity();
 		transform.setOrigin(localRootBodyPosition);
@@ -193,7 +194,7 @@ public:
 			float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle
 
 			transform.setIdentity();
-			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(rootBodyRadius+0.5*legLength)), btScalar(footHeight), btScalar(footYUnitPosition*(rootBodyRadius+0.5*legLength)));
+			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+0.5*gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition*(gRootBodyRadius+0.5*gLegLength)));
 			transform.setOrigin(legCOM);
 
 			// thigh
@@ -204,7 +205,7 @@ public:
 
 			// shin
 			transform.setIdentity();
-			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(footHeight-0.5*foreLegLength), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
 			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
 
 			//
@@ -213,7 +214,7 @@ public:
 
 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
-			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*rootBodyRadius)));
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*gRootBodyRadius)));
 			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
 			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
@@ -223,7 +224,7 @@ public:
 
 			// knee joints
 			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
-			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(0.), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(0.), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
 			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			localC = getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
@@ -241,7 +242,7 @@ public:
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
 			m_bodies[i]->setUserPointer(this);
-			m_body_index_map.insert(std::pair<void*,int>(m_bodies[i],i));
+			m_bodyIndexMap.insert(std::pair<void*,int>(m_bodies[i],i));
 
 		}
 	}
@@ -272,19 +273,19 @@ public:
 	btTypedConstraint** getJoints() {return &m_joints[0];}
 
 	void setTouchSensor(void* bodyPointer){
-		m_touch_sensors[m_body_index_map.at(bodyPointer)] = true;
+		m_touchSensors[m_bodyIndexMap.at(bodyPointer)] = true;
 	}
 
 	void clearTouchSensors(){
 		for(int i = 0 ; i < BODYPART_COUNT;i++){
-			m_touch_sensors[i] = false;
+			m_touchSensors[i] = false;
 		}
 	}
 
-	bool getTouchSensor(int i){ return m_touch_sensors[i];}
+	bool getTouchSensor(int i){ return m_touchSensors[i];}
 
 	const float* getSensoryMotorWeights() const {
-		return m_sensory_motor_weights;
+		return m_sensoryMotorWeights;
 	}
 };
 
@@ -366,6 +367,60 @@ void NN3DWalkers::initPhysics()
 			slider);
 	}
 	
+	{ // create a slider to change the root body radius
+		SliderParams slider("Root body radius", &gRootBodyRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
+	{ // create a slider to change the root body height
+		SliderParams slider("Root body height", &gRootBodyHeight);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
+	{ // create a slider to change the leg radius
+		SliderParams slider("Leg radius", &gLegRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
+	{ // create a slider to change the leg length
+		SliderParams slider("Leg length", &gLegLength);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
+	{ // create a slider to change the fore leg radius
+			SliderParams slider("Fore Leg radius", &gForeLegRadius);
+			slider.m_minVal = 0.01f;
+			slider.m_maxVal = 10;
+			slider.m_clampToNotches = false;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+				slider);
+		}
+
+		{ // create a slider to change the fore leg length
+			SliderParams slider("Fore Leg length", &gForeLegLength);
+			slider.m_minVal = 0.01f;
+			slider.m_maxVal = 10;
+			slider.m_clampToNotches = false;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+				slider);
+		}
+
 
 	// Setup a big ground box
 	{

From a5b55e8fbf9a3ebceed8a5bae670c4d11f5f4039 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Tue, 5 Jul 2016 18:57:51 +0200
Subject: [PATCH 08/22] Randomize the dimensions of the creature within a
 certain dimension range.

---
 examples/Evolution/NN3DWalkers.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index d6f993bbc..8d92ee3c8 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -105,6 +105,8 @@ static NN3DWalkers* nn3DWalkers = NULL;
 void* GROUND_ID = (void*)1;
 bool RANDOM_MOVEMENT = false;
 
+bool RANDOM_DIMENSIONS = true;
+
 #define NUM_LEGS 6
 #define BODYPART_COUNT (2 * NUM_LEGS + 1)
 #define JOINT_COUNT (BODYPART_COUNT - 1)
@@ -436,6 +438,19 @@ void NN3DWalkers::initPhysics()
 
 	for(int i = 0; i < 5 ; i++){
 		for(int j = 0; j < 5; j++){
+
+			float maxDimension = 0.3f;
+
+			if(RANDOM_DIMENSIONS){
+				// randomize the dimensions
+				gRootBodyRadius  = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+				gRootBodyHeight = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+				gLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+				gLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+				gForeLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+				gForeLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			}
+
 			// Spawn one walker
 			btVector3 spacing(10.0f,0.8f,10.0f);
 			btVector3 startOffset(spacing * btVector3(i,0,j));

From 46af3ddc54336152a9ca44de987f112516753bb4 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Sun, 31 Jul 2016 16:07:38 +0200
Subject: [PATCH 09/22] Normal settings.

---
 examples/Evolution/NN3DWalkers.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 8d92ee3c8..af9a4162b 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -34,6 +34,8 @@ class btDefaultCollisionConfiguration;
 
 //TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
 
+//TODO: How to detect perpetually interpenetrating btRigidBodies? (Maybe contactpoints can tell us something)
+
 btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 point);
 btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 point);
 
@@ -105,7 +107,7 @@ static NN3DWalkers* nn3DWalkers = NULL;
 void* GROUND_ID = (void*)1;
 bool RANDOM_MOVEMENT = false;
 
-bool RANDOM_DIMENSIONS = true;
+bool RANDOM_DIMENSIONS = false;
 
 #define NUM_LEGS 6
 #define BODYPART_COUNT (2 * NUM_LEGS + 1)
@@ -117,7 +119,7 @@ class NNWalker
 	btCollisionShape*	m_shapes[BODYPART_COUNT];
 	btRigidBody*		m_bodies[BODYPART_COUNT];
 	btTypedConstraint*	m_joints[JOINT_COUNT];
-	std::map<void*,int>			m_bodyIndexMap;
+	std::map<void*,int>	m_bodyTouchSensorIndexMap;
 	bool 				m_touchSensors[BODYPART_COUNT];
 	float 				m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
 
@@ -244,7 +246,7 @@ public:
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
 			m_bodies[i]->setUserPointer(this);
-			m_bodyIndexMap.insert(std::pair<void*,int>(m_bodies[i],i));
+			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[i],i));
 
 		}
 	}
@@ -275,7 +277,7 @@ public:
 	btTypedConstraint** getJoints() {return &m_joints[0];}
 
 	void setTouchSensor(void* bodyPointer){
-		m_touchSensors[m_bodyIndexMap.at(bodyPointer)] = true;
+		m_touchSensors[m_bodyTouchSensorIndexMap.at(bodyPointer)] = true;
 	}
 
 	void clearTouchSensors(){
@@ -439,7 +441,7 @@ void NN3DWalkers::initPhysics()
 	for(int i = 0; i < 5 ; i++){
 		for(int j = 0; j < 5; j++){
 
-			float maxDimension = 0.3f;
+			float maxDimension = 0.2f;
 
 			if(RANDOM_DIMENSIONS){
 				// randomize the dimensions

From dcfa5a3ee235b3403875587943af2ef6636022c9 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <be.ellenberger@gmail.com>
Date: Tue, 2 Aug 2016 20:38:36 +0200
Subject: [PATCH 10/22] Clear gContactProcessedCallback on exiting physics.

---
 examples/Evolution/NN3DWalkers.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index af9a4162b..4ecdc9a2b 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -545,6 +545,8 @@ bool NN3DWalkers::keyboardCallback(int key, int state)
 void NN3DWalkers::exitPhysics()
 {
 
+	gContactProcessedCallback = NULL; // clear contact processed callback on exiting
+
 	int i;
 
 	for (i=0;i<m_walkers.size();i++)

From d9a2113b28c052f02436c7a935cabacd869822ce Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Tue, 6 Sep 2016 22:30:46 +0200
Subject: [PATCH 11/22] Implement simple body part drop strategy to resolve
 body interpenetrations

---
 examples/Evolution/NN3DWalkers.cpp | 147 +++++++++++++++++++----------
 1 file changed, 98 insertions(+), 49 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 4ecdc9a2b..f40ad16e1 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -52,7 +52,9 @@ static btScalar gLegLength = 0.45f;
 static btScalar gForeLegLength = 0.75f;
 static btScalar gForeLegRadius = 0.08f;
 
-class NN3DWalkers : public CommonRigidBodyBase
+void* GROUND_ID = (void*)1;
+
+class NN3DWalkersExample : public CommonRigidBodyBase
 {
 	btScalar m_Time;
 	btScalar m_targetAccumulator;
@@ -63,7 +65,7 @@ class NN3DWalkers : public CommonRigidBodyBase
 	
 	
 public:
-	NN3DWalkers(struct GUIHelperInterface* helper)
+	NN3DWalkersExample(struct GUIHelperInterface* helper)
 	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0)
 	{
 
@@ -72,7 +74,7 @@ public:
 	
 	virtual void exitPhysics();
 
-	virtual ~NN3DWalkers()
+	virtual ~NN3DWalkersExample()
 	{
 	}
 	
@@ -82,6 +84,44 @@ public:
 	
 	void setMotorTargets(btScalar deltaTime);
 	
+	bool detectCollisions(){
+		bool collisionDetected = false;
+		if(m_dynamicsWorld){
+			m_dynamicsWorld->performDiscreteCollisionDetection(); // let the collisions be calculated
+		}
+
+		int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold =  m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
+			const btCollisionObject* obA = contactManifold->getBody0();
+			const btCollisionObject* obB = contactManifold->getBody1();
+
+			if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){
+
+				int numContacts = contactManifold->getNumContacts();
+				for (int j=0;j<numContacts;j++)
+				{
+					collisionDetected = true;
+					btManifoldPoint& pt = contactManifold->getContactPoint(j);
+					if (pt.getDistance()<0.f)
+					{
+						const btVector3& ptA = pt.getPositionWorldOnA();
+						const btVector3& ptB = pt.getPositionWorldOnB();
+						const btVector3& normalOnB = pt.m_normalWorldOnB;
+
+						if(m_dynamicsWorld->getDebugDrawer()){
+							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.));
+							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.));
+						}
+					}
+				}
+			}
+		}
+
+		return collisionDetected;
+	}
+
 	void resetCamera()
 	{
 		float dist = 11;
@@ -94,7 +134,7 @@ public:
 	virtual void renderScene();
 };
 
-static NN3DWalkers* nn3DWalkers = NULL;
+static NN3DWalkersExample* nn3DWalkers = NULL;
 
 #ifndef SIMD_PI_4
 #define SIMD_PI_4     0.5 * SIMD_HALF_PI
@@ -104,7 +144,6 @@ static NN3DWalkers* nn3DWalkers = NULL;
 #define SIMD_PI_8     0.25 * SIMD_HALF_PI
 #endif
 
-void* GROUND_ID = (void*)1;
 bool RANDOM_MOVEMENT = false;
 
 bool RANDOM_DIMENSIONS = false;
@@ -135,8 +174,6 @@ class NNWalker
 		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,shape,localInertia);
 		btRigidBody* body = new btRigidBody(rbInfo);
 
-		m_ownerWorld->addRigidBody(body);
-
 		return body;
 	}
 
@@ -156,7 +193,6 @@ public:
 
 		//
 		// Setup geometry
-		//
 		m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule
 		int i;
 		for ( i=0; i<NUM_LEGS; i++)
@@ -167,7 +203,6 @@ public:
 
 		//
 		// Setup rigid bodies
-		//
 		float rootAboveGroundHeight = gForeLegLength;
 		btTransform bodyOffset; bodyOffset.setIdentity();
 		bodyOffset.setOrigin(positionOffset);		
@@ -177,13 +212,11 @@ public:
 		btTransform transform;
 		transform.setIdentity();
 		transform.setOrigin(localRootBodyPosition);
-		if (bFixed) // if fixed body
-		{
-			m_bodies[0] = localCreateRigidBody(btScalar(0.), bodyOffset*transform, m_shapes[0]);
-		} else
-		{
-			m_bodies[0] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[0]);
-		}
+
+		m_bodies[0] = localCreateRigidBody(btScalar(bFixed?0.:1.), bodyOffset*transform, m_shapes[0]);
+		m_ownerWorld->addRigidBody(m_bodies[0]);
+		m_bodies[0]->setUserPointer(this);
+		m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[0], 0));
 
 		btHingeConstraint* hingeC;
 		//btConeTwistConstraint* coneC;
@@ -206,15 +239,15 @@ public:
 			btVector3 kneeAxis = legDirection.cross(vUp);			
 			transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
 			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
+			m_bodies[1+2*i]->setUserPointer(this);
+			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[1+2*i],1+2*i));
 
 			// shin
 			transform.setIdentity();
 			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
 			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
-
-			//
-			// Setup the constraints
-			//
+			m_bodies[2+2*i]->setUserPointer(this);
+			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[2+2*i],2+2*i));
 
 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
@@ -224,7 +257,6 @@ public:
 			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
 			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
 			m_joints[2*i] = hingeC;
-			m_ownerWorld->addConstraint(m_joints[2*i], true);
 
 			// knee joints
 			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
@@ -235,7 +267,25 @@ public:
 			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
 			hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
 			m_joints[1+2*i] = hingeC;
-			m_ownerWorld->addConstraint(m_joints[1+2*i], true);
+
+			m_ownerWorld->addRigidBody(m_bodies[1+2*i]); // add thigh bone
+
+			m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root
+
+			if(nn3DWalkers->detectCollisions()){ // if thigh bone causes collision, remove it again
+				m_ownerWorld->removeRigidBody(m_bodies[1+2*i]);
+				m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root
+			}
+			else{
+
+				m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone
+				m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thig
+
+				if(nn3DWalkers->detectCollisions()){ // if shin bone causes collision, remove it again
+					m_ownerWorld->removeRigidBody(m_bodies[2+2*i]);
+					m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh
+				}
+			}
 		}
 
 		// Setup some damping on the m_bodies
@@ -245,9 +295,6 @@ public:
 			m_bodies[i]->setDeactivationTime(0.8);
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
-			m_bodies[i]->setUserPointer(this);
-			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[i],i));
-
 		}
 	}
 
@@ -297,9 +344,10 @@ public:
 
 void legMotorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 {
-	NN3DWalkers* motorDemo = (NN3DWalkers*)world->getWorldUserInfo();
+	NN3DWalkersExample* motorDemo = (NN3DWalkersExample*)world->getWorldUserInfo();
 
 	motorDemo->setMotorTargets(timeStep);
+	nn3DWalkers->detectCollisions();
 	
 }
 
@@ -315,8 +363,9 @@ bool legContactProcessedCallback(btManifoldPoint& cp,
 	if (ID2 != GROUND_ID || ID1 != GROUND_ID) {
 	    // Make a circle with a 0.9 radius at (0,0,0)
 	    // with RGB color (1,0,0).
-		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL)
+		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL){
 			nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
+		}
 
 		if(ID1 != GROUND_ID){
 			((NNWalker*)ID1)->setTouchSensor(o1);
@@ -331,7 +380,7 @@ bool legContactProcessedCallback(btManifoldPoint& cp,
 
 
 
-void NN3DWalkers::initPhysics()
+void NN3DWalkersExample::initPhysics()
 {
 	gContactProcessedCallback = legContactProcessedCallback;
 
@@ -408,22 +457,22 @@ void NN3DWalkers::initPhysics()
 	}
 
 	{ // create a slider to change the fore leg radius
-			SliderParams slider("Fore Leg radius", &gForeLegRadius);
-			slider.m_minVal = 0.01f;
-			slider.m_maxVal = 10;
-			slider.m_clampToNotches = false;
-			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-				slider);
-		}
+		SliderParams slider("Fore Leg radius", &gForeLegRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
 
-		{ // create a slider to change the fore leg length
-			SliderParams slider("Fore Leg length", &gForeLegLength);
-			slider.m_minVal = 0.01f;
-			slider.m_maxVal = 10;
-			slider.m_clampToNotches = false;
-			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-				slider);
-		}
+	{ // create a slider to change the fore leg length
+		SliderParams slider("Fore Leg length", &gForeLegLength);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
 
 
 	// Setup a big ground box
@@ -464,13 +513,13 @@ void NN3DWalkers::initPhysics()
 }
 
 
-void NN3DWalkers::spawnWalker(const btVector3& startOffset, bool bFixed)
+void NN3DWalkersExample::spawnWalker(const btVector3& startOffset, bool bFixed)
 {
 	NNWalker* walker = new NNWalker(m_dynamicsWorld, startOffset, bFixed);
 	m_walkers.push_back(walker);
 }
 
-void NN3DWalkers::setMotorTargets(btScalar deltaTime)
+void NN3DWalkersExample::setMotorTargets(btScalar deltaTime)
 {
 
 	float ms = deltaTime*1000000.;
@@ -521,7 +570,7 @@ void NN3DWalkers::setMotorTargets(btScalar deltaTime)
 	}
 }
 
-bool NN3DWalkers::keyboardCallback(int key, int state)
+bool NN3DWalkersExample::keyboardCallback(int key, int state)
 {
 	switch (key)
 	{
@@ -542,7 +591,7 @@ bool NN3DWalkers::keyboardCallback(int key, int state)
 
 
 
-void NN3DWalkers::exitPhysics()
+void NN3DWalkersExample::exitPhysics()
 {
 
 	gContactProcessedCallback = NULL; // clear contact processed callback on exiting
@@ -558,7 +607,7 @@ void NN3DWalkers::exitPhysics()
 	CommonRigidBodyBase::exitPhysics();
 }
 
-void NN3DWalkers::renderScene()
+void NN3DWalkersExample::renderScene()
 	{
 		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
 
@@ -569,7 +618,7 @@ void NN3DWalkers::renderScene()
 
 class CommonExampleInterface*    ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
-	nn3DWalkers = new NN3DWalkers(options.m_guiHelper);
+	nn3DWalkers = new NN3DWalkersExample(options.m_guiHelper);
 	return nn3DWalkers;
 }
 

From 88edbf852421debb298e797f9d23bd4a6bf62bbd Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Sun, 11 Sep 2016 22:25:22 +0200
Subject: [PATCH 12/22] Implement basic evoluationary algorithm.

---
 examples/Evolution/NN3DWalkers.cpp        | 667 ++++++++++++++++------
 examples/Evolution/NN3DWalkers.h          |   4 +-
 examples/Utils/b3ReferenceFrameHelper.hpp |  56 ++
 3 files changed, 536 insertions(+), 191 deletions(-)
 create mode 100755 examples/Utils/b3ReferenceFrameHelper.hpp

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index f40ad16e1..4110f0d32 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -1,6 +1,6 @@
 /*
-Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
-Motor Demo
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "NN3DWalkers.h"
 #include <map>
 
@@ -28,22 +27,12 @@ class btCollisionDispatcher;
 class btConstraintSolver;
 struct btCollisionAlgorithmCreateFunc;
 class btDefaultCollisionConfiguration;
+class NNWalker;
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
-//TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class
-
-//TODO: How to detect perpetually interpenetrating btRigidBodies? (Maybe contactpoints can tell us something)
-
-btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 point);
-btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 point);
-
-btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis);
-btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis);
-
-btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform);
-btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform);
+#include "../Utils/b3ReferenceFrameHelper.hpp"
 
 static btScalar gRootBodyRadius  = 0.25f;
 static btScalar gRootBodyHeight = 0.1f;
@@ -52,6 +41,63 @@ static btScalar gLegLength = 0.45f;
 static btScalar gForeLegLength = 0.75f;
 static btScalar gForeLegRadius = 0.08f;
 
+#ifndef SIMD_PI_4
+#define SIMD_PI_4     0.5 * SIMD_HALF_PI
+#endif
+
+#ifndef SIMD_PI_8
+#define SIMD_PI_8     0.25 * SIMD_HALF_PI
+#endif
+
+#ifndef RANDOM_MOVEMENT
+#define RANDOM_MOVEMENT false
+#endif
+
+#ifndef RANDOMIZE_DIMENSIONS
+#define RANDOMIZE_DIMENSIONS false
+#endif
+
+#ifndef NUM_WALKERS
+#define NUM_WALKERS 50
+#endif
+
+#ifndef NUM_PARALLEL_EVALUATIONS
+#define NUM_PARALLEL_EVALUATIONS 1
+#endif
+
+#ifndef EVALUATION_TIME
+#define EVALUATION_TIME 10 // s
+#endif
+
+#ifndef REAP_QTY
+#define REAP_QTY 0.3f // number of walkers reaped based on their bad performance
+#endif
+
+#ifndef SOW_CROSSOVER_QTY
+#define SOW_CROSSOVER_QTY 0.2f // this means REAP_QTY-SOW_CROSSOVER_QTY = NEW_RANDOM_BREED_QTY
+#endif
+
+#ifndef SOW_ELITE_QTY
+#define SOW_ELITE_QTY 0.2f // number of walkers kept using an elitist strategy
+#endif
+
+#ifndef SOW_MUTATION_QTY
+#define SOW_MUTATION_QTY 0.5f // SOW_ELITE_QTY + SOW_MUTATION_QTY + REAP_QTY = 1
+#endif
+
+#ifndef MUTATION_RATE
+#define MUTATION_RATE 0.5f // the mutation rate of for the walker with the worst performance
+#endif
+
+#ifndef SOW_ELITE_PARTNER
+#define SOW_ELITE_PARTNER 0.8f
+#endif
+
+#define NUM_LEGS 6
+#define BODYPART_COUNT (2 * NUM_LEGS + 1)
+#define JOINT_COUNT (BODYPART_COUNT - 1)
+#define DRAW_INTERPENETRATIONS false
+
 void* GROUND_ID = (void*)1;
 
 class NN3DWalkersExample : public CommonRigidBodyBase
@@ -60,23 +106,24 @@ class NN3DWalkersExample : public CommonRigidBodyBase
 	btScalar m_targetAccumulator;
 	btScalar m_targetFrequency;
 	btScalar m_motorStrength;
+	int 	 m_evaluationsQty;
+	int		 m_nextReaped;
 	
-	btAlignedObjectArray<class NNWalker*> m_walkers;
-	
+	btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;
 	
 public:
 	NN3DWalkersExample(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0)
+	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0),m_evaluationsQty(0),m_nextReaped(0)
 	{
-
 	}
-	void initPhysics();
-	
-	virtual void exitPhysics();
 
 	virtual ~NN3DWalkersExample()
 	{
 	}
+
+	void initPhysics();
+	
+	virtual void exitPhysics();
 	
 	void spawnWalker(const btVector3& startOffset, bool bFixed);
 	
@@ -84,43 +131,7 @@ public:
 	
 	void setMotorTargets(btScalar deltaTime);
 	
-	bool detectCollisions(){
-		bool collisionDetected = false;
-		if(m_dynamicsWorld){
-			m_dynamicsWorld->performDiscreteCollisionDetection(); // let the collisions be calculated
-		}
-
-		int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
-		for (int i=0;i<numManifolds;i++)
-		{
-			btPersistentManifold* contactManifold =  m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
-			const btCollisionObject* obA = contactManifold->getBody0();
-			const btCollisionObject* obB = contactManifold->getBody1();
-
-			if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){
-
-				int numContacts = contactManifold->getNumContacts();
-				for (int j=0;j<numContacts;j++)
-				{
-					collisionDetected = true;
-					btManifoldPoint& pt = contactManifold->getContactPoint(j);
-					if (pt.getDistance()<0.f)
-					{
-						const btVector3& ptA = pt.getPositionWorldOnA();
-						const btVector3& ptB = pt.getPositionWorldOnB();
-						const btVector3& normalOnB = pt.m_normalWorldOnB;
-
-						if(m_dynamicsWorld->getDebugDrawer()){
-							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.));
-							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.));
-						}
-					}
-				}
-			}
-		}
-
-		return collisionDetected;
-	}
+	bool detectCollisions();
 
 	void resetCamera()
 	{
@@ -132,35 +143,52 @@ public:
 	}
 
 	virtual void renderScene();
+
+	// Evaluation
+
+	void update(const double timeSinceLastTick);
+
+	void updateEvaluations(const double timeSinceLastTick);
+
+	void scheduleEvaluations();
+
+	// Reaper
+
+	void rateEvaluations();
+
+	void reap();
+
+	void sow();
+
+	void crossover(NNWalker* mother, NNWalker* father, NNWalker* offspring);
+
+	void mutate(NNWalker* mutant, float mutationRate);
+
+	NNWalker* getRandomElite();
+
+	NNWalker* getRandomNonElite();
+
+	NNWalker* getNextReaped();
+
 };
 
 static NN3DWalkersExample* nn3DWalkers = NULL;
 
-#ifndef SIMD_PI_4
-#define SIMD_PI_4     0.5 * SIMD_HALF_PI
-#endif
-
-#ifndef SIMD_PI_8
-#define SIMD_PI_8     0.25 * SIMD_HALF_PI
-#endif
-
-bool RANDOM_MOVEMENT = false;
-
-bool RANDOM_DIMENSIONS = false;
-
-#define NUM_LEGS 6
-#define BODYPART_COUNT (2 * NUM_LEGS + 1)
-#define JOINT_COUNT (BODYPART_COUNT - 1)
-
 class NNWalker
 {
 	btDynamicsWorld*	m_ownerWorld;
 	btCollisionShape*	m_shapes[BODYPART_COUNT];
 	btRigidBody*		m_bodies[BODYPART_COUNT];
+	btTransform			m_bodyRelativeTransforms[BODYPART_COUNT];
 	btTypedConstraint*	m_joints[JOINT_COUNT];
 	std::map<void*,int>	m_bodyTouchSensorIndexMap;
 	bool 				m_touchSensors[BODYPART_COUNT];
-	float 				m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
+	btScalar			m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
+
+	bool				m_inEvaluation;
+	btScalar			m_evaluationTime;
+	bool				m_reaped;
+	btVector3			m_startPosition;
 
 	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
 	{
@@ -179,17 +207,24 @@ class NNWalker
 
 
 public:
-	NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
-		: m_ownerWorld (ownerWorld)
-	{
-		btVector3 vUp(0, 1, 0); // up in local reference frame
 
+	void randomizeSensoryMotorWeights(){
 		//initialize random weights
 		for(int i = 0;i < BODYPART_COUNT;i++){
 			for(int j = 0;j < JOINT_COUNT;j++){
 				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
 			}
 		}
+	}
+
+	NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
+		: m_ownerWorld (ownerWorld), m_inEvaluation(false), m_evaluationTime(0), m_reaped(false)
+	{
+		btVector3 vUp(0, 1, 0); // up in local reference frame
+
+		NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();
+
+		randomizeSensoryMotorWeights();
 
 		//
 		// Setup geometry
@@ -203,18 +238,21 @@ public:
 
 		//
 		// Setup rigid bodies
-		float rootAboveGroundHeight = gForeLegLength;
+		btScalar rootAboveGroundHeight = gForeLegLength;
 		btTransform bodyOffset; bodyOffset.setIdentity();
 		bodyOffset.setOrigin(positionOffset);		
 
 		// root body
-		btVector3 localRootBodyPosition = btVector3(btScalar(0.), btScalar(rootAboveGroundHeight), btScalar(0.)); // root body position in local reference frame
+		btVector3 localRootBodyPosition = btVector3(btScalar(0.), rootAboveGroundHeight, btScalar(0.)); // root body position in local reference frame
 		btTransform transform;
 		transform.setIdentity();
 		transform.setOrigin(localRootBodyPosition);
 
+		btTransform originTransform = transform;
+
 		m_bodies[0] = localCreateRigidBody(btScalar(bFixed?0.:1.), bodyOffset*transform, m_shapes[0]);
 		m_ownerWorld->addRigidBody(m_bodies[0]);
+		m_bodyRelativeTransforms[0] = btTransform();
 		m_bodies[0]->setUserPointer(this);
 		m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[0], 0));
 
@@ -224,7 +262,7 @@ public:
 		btTransform localA, localB, localC;
 
 		// legs
-		for ( i=0; i<NUM_LEGS; i++)
+		for (i = 0; i < NUM_LEGS; i++)
 		{
 			float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
 			float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle
@@ -239,6 +277,7 @@ public:
 			btVector3 kneeAxis = legDirection.cross(vUp);			
 			transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
 			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
+			m_bodyRelativeTransforms[1+2*i] = transform;
 			m_bodies[1+2*i]->setUserPointer(this);
 			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[1+2*i],1+2*i));
 
@@ -246,13 +285,14 @@ public:
 			transform.setIdentity();
 			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
 			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
+			m_bodyRelativeTransforms[2+2*i] = transform;
 			m_bodies[2+2*i]->setUserPointer(this);
 			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[2+2*i],2+2*i));
 
 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
 			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*gRootBodyRadius)));
-			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
+			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
 			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
 			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
@@ -261,8 +301,8 @@ public:
 			// knee joints
 			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
 			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(0.), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
-			localB = getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
-			localC = getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
+			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
+			localC = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
 			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
 			hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
@@ -272,16 +312,16 @@ public:
 
 			m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root
 
-			if(nn3DWalkers->detectCollisions()){ // if thigh bone causes collision, remove it again
+			if(nnWalkersDemo->detectCollisions()){ // if thigh bone causes collision, remove it again
 				m_ownerWorld->removeRigidBody(m_bodies[1+2*i]);
 				m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root
 			}
 			else{
 
 				m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone
-				m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thig
+				m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thigh
 
-				if(nn3DWalkers->detectCollisions()){ // if shin bone causes collision, remove it again
+				if(nnWalkersDemo->detectCollisions()){ // if shin bone causes collision, remove it again
 					m_ownerWorld->removeRigidBody(m_bodies[2+2*i]);
 					m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh
 				}
@@ -296,6 +336,8 @@ public:
 			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
 			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
 		}
+
+		removeFromWorld(); // it should not yet be in the world
 	}
 
 	virtual	~NNWalker ()
@@ -321,7 +363,9 @@ public:
 		}
 	}
 
-	btTypedConstraint** getJoints() {return &m_joints[0];}
+	btTypedConstraint** getJoints() {
+		return &m_joints[0];
+	}
 
 	void setTouchSensor(void* bodyPointer){
 		m_touchSensors[m_bodyTouchSensorIndexMap.at(bodyPointer)] = true;
@@ -333,26 +377,119 @@ public:
 		}
 	}
 
-	bool getTouchSensor(int i){ return m_touchSensors[i];}
+	bool getTouchSensor(int i){
+		return m_touchSensors[i];
+	}
 
-	const float* getSensoryMotorWeights() const {
+	btScalar* getSensoryMotorWeights() {
 		return m_sensoryMotorWeights;
 	}
+
+	void addToWorld() {
+		int i;
+		// add all bodies and shapes
+		for ( i = 0; i < BODYPART_COUNT; ++i)
+		{
+			m_ownerWorld->addRigidBody(m_bodies[i]);
+		}
+
+		// add all constraints
+		for ( i = 0; i < JOINT_COUNT; ++i)
+		{
+			m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies
+		}
+		m_startPosition = getPosition();
+	}
+
+	void removeFromWorld(){
+		int i;
+
+		// Remove all constraints
+		for ( i = 0; i < JOINT_COUNT; ++i)
+		{
+			m_ownerWorld->removeConstraint(m_joints[i]);
+		}
+
+		// Remove all bodies and shapes
+		for ( i = 0; i < BODYPART_COUNT; ++i)
+		{
+			m_ownerWorld->removeRigidBody(m_bodies[i]);
+		}
+	}
+
+	btVector3 getPosition() const {
+		btVector3 finalPosition(0,0,0);
+
+		for(int i = 0; i < BODYPART_COUNT;i++)
+		{
+			//b3Printf(" position (%f,%f,%f)",m_bodies[i]->getCenterOfMassPosition().x(),m_bodies[i]->getCenterOfMassPosition().y(),m_bodies[i]->getCenterOfMassPosition().z());
+
+			finalPosition += m_bodies[i]->getCenterOfMassPosition();
+		}
+
+		finalPosition /= BODYPART_COUNT;
+		return finalPosition;
+	}
+
+	btScalar getDistanceFitness() const
+	{
+		btScalar distance = 0;
+
+		distance = (getPosition() - m_startPosition).length2();
+
+		return distance;
+	}
+
+	btScalar getFitness() const
+	{
+		return getDistanceFitness(); // for now it is only distance
+	}
+
+	void resetAt(btVector3 position) {
+		btTransform resetPosition(btQuaternion(), position);
+		for (int i = 0; i < BODYPART_COUNT; ++i)
+		{
+			m_bodies[i]->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]);
+			if(m_bodies[i]->getMotionState()){
+				m_bodies[i]->getMotionState()->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]);
+			}
+			m_bodies[i]->clearForces();
+			m_bodies[i]->setAngularVelocity(btVector3(0,0,0));
+			m_bodies[i]->setLinearVelocity(btVector3(0,0,0));
+
+		}
+
+		clearTouchSensors();
+	}
+
+	double getEvaluationTime() const {
+		return m_evaluationTime;
+	}
+
+	void setEvaluationTime(double evaluationTime) {
+		m_evaluationTime = evaluationTime;
+	}
+
+	bool isInEvaluation() const {
+		return m_inEvaluation;
+	}
+
+	void setInEvaluation(bool inEvaluation) {
+		m_inEvaluation = inEvaluation;
+	}
+
+	bool isReaped() const {
+		return m_reaped;
+	}
+
+	void setReaped(bool reaped) {
+		m_reaped = reaped;
+	}
 };
 
+void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep);
 
-
-void legMotorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
-{
-	NN3DWalkersExample* motorDemo = (NN3DWalkersExample*)world->getWorldUserInfo();
-
-	motorDemo->setMotorTargets(timeStep);
-	nn3DWalkers->detectCollisions();
-	
-}
-
-bool legContactProcessedCallback(btManifoldPoint& cp,
-                                void* body0, void* body1)
+bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
 {
     btCollisionObject* o1 = static_cast<btCollisionObject*>(body0);
     btCollisionObject* o2 = static_cast<btCollisionObject*>(body1);
@@ -378,8 +515,6 @@ bool legContactProcessedCallback(btManifoldPoint& cp,
     return false;
 }
 
-
-
 void NN3DWalkersExample::initPhysics()
 {
 	gContactProcessedCallback = legContactProcessedCallback;
@@ -392,7 +527,7 @@ void NN3DWalkersExample::initPhysics()
 
 	createEmptyDynamicsWorld();
 
-	m_dynamicsWorld->setInternalTickCallback(legMotorPreTickCallback,this,true);
+	m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
 	m_targetFrequency = 3;
@@ -487,91 +622,78 @@ void NN3DWalkersExample::initPhysics()
 		ground->setUserPointer(GROUND_ID);
 	}
 
-	for(int i = 0; i < 5 ; i++){
-		for(int j = 0; j < 5; j++){
-
+	for(int i = 0; i < NUM_WALKERS ; i++){
+		if(RANDOMIZE_DIMENSIONS){
 			float maxDimension = 0.2f;
 
-			if(RANDOM_DIMENSIONS){
-				// randomize the dimensions
-				gRootBodyRadius  = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-				gRootBodyHeight = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-				gLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-				gLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-				gForeLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-				gForeLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
-			}
-
-			// Spawn one walker
-			btVector3 spacing(10.0f,0.8f,10.0f);
-			btVector3 startOffset(spacing * btVector3(i,0,j));
-			spawnWalker(startOffset, false);
+			// randomize the dimensions
+			gRootBodyRadius  = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			gRootBodyHeight = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			gLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			gLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			gForeLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
+			gForeLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
 		}
-	}
 
-	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+		// Spawn one walker
+		btVector3 offset(0,0,0);
+		spawnWalker(offset, false);
+	}
 }
 
 
 void NN3DWalkersExample::spawnWalker(const btVector3& startOffset, bool bFixed)
 {
 	NNWalker* walker = new NNWalker(m_dynamicsWorld, startOffset, bFixed);
-	m_walkers.push_back(walker);
+	m_walkersInPopulation.push_back(walker);
 }
 
-void NN3DWalkersExample::setMotorTargets(btScalar deltaTime)
+bool NN3DWalkersExample::detectCollisions()
 {
+	bool collisionDetected = false;
+	if(m_dynamicsWorld){
+		m_dynamicsWorld->performDiscreteCollisionDetection(); // let the collisions be calculated
+	}
 
-	float ms = deltaTime*1000000.;
-	float minFPS = 1000000.f/60.f;
-	if (ms > minFPS)
-		ms = minFPS;
-
-	m_Time += ms;
-
-	m_targetAccumulator +=ms;
-
-	if(m_targetAccumulator >= 1000000.0f /((double)m_targetFrequency))
+	int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
+	for (int i = 0;i < numManifolds;i++)
 	{
-		m_targetAccumulator = 0;
-		//
-		// set per-frame sinusoidal position targets using angular motor (hacky?)
-		//
-		for (int r=0; r<m_walkers.size(); r++)
-		{
-			for (int i=0; i<2*NUM_LEGS; i++)
+		btPersistentManifold* contactManifold =  m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
+		const btCollisionObject* obA = contactManifold->getBody0();
+		const btCollisionObject* obB = contactManifold->getBody1();
+
+		if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
 			{
-				btScalar targetAngle = 0;
-				btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkers[r]->getJoints()[i]);
+				collisionDetected = true;
+				btManifoldPoint& pt = contactManifold->getContactPoint(j);
+				if (pt.getDistance()<0.f)
+				{
+					const btVector3& ptA = pt.getPositionWorldOnA();
+					const btVector3& ptB = pt.getPositionWorldOnB();
+					const btVector3& normalOnB = pt.m_normalWorldOnB;
 
-				if(RANDOM_MOVEMENT){
-					targetAngle   = ((double) rand() / (RAND_MAX));//0.5 * (1 + sin(2 * SIMD_PI * fTargetPercent+ i* SIMD_PI/NUM_LEGS));
-				}
-				else{
-
-					// accumulate sensor inputs with weights
-					for(int j = 0; j < JOINT_COUNT;j++){
-						targetAngle += m_walkers[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkers[r]->getTouchSensor(i);
+					if(!DRAW_INTERPENETRATIONS){
+						return collisionDetected;
 					}
 
-					// apply the activation function
-					targetAngle = (tanh(targetAngle)+1.0f)*0.5f;
+					if(m_dynamicsWorld->getDebugDrawer()){
+						m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.));
+						m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.));
+					}
 				}
-				btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
-				btScalar currentAngle      = hingeC->getHingeAngle();
-				btScalar angleError  = targetLimitAngle - currentAngle;
-				btScalar desiredAngularVel = 1000000.f * angleError/ms;
-				hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength);
 			}
-
-			// clear sensor signals after usage
-			m_walkers[r]->clearTouchSensors();
 		}
 	}
+
+	return collisionDetected;
 }
 
 bool NN3DWalkersExample::keyboardCallback(int key, int state)
 {
+	//TODO: Redesign the button setups
 	switch (key)
 	{
 	case '[':
@@ -589,8 +711,6 @@ bool NN3DWalkersExample::keyboardCallback(int key, int state)
 	return false;
 }
 
-
-
 void NN3DWalkersExample::exitPhysics()
 {
 
@@ -598,9 +718,9 @@ void NN3DWalkersExample::exitPhysics()
 
 	int i;
 
-	for (i=0;i<m_walkers.size();i++)
+	for (i = 0;i < m_walkersInPopulation.size();i++)
 	{
-		NNWalker* walker = m_walkers[i];
+		NNWalker* walker = m_walkersInPopulation[i];
 		delete walker;
 	}
 
@@ -616,39 +736,208 @@ void NN3DWalkersExample::renderScene()
 		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
 	}
 
-class CommonExampleInterface*    ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
+class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
 	nn3DWalkers = new NN3DWalkersExample(options.m_guiHelper);
 	return nn3DWalkers;
 }
 
-
-btVector3 getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 point) {
-	  return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame
+bool fitnessComparator (const NNWalker* a, const NNWalker* b)
+{
+    return a->getFitness() > b->getFitness();
 }
 
-btVector3 getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 point) {
-	  return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame
+void NN3DWalkersExample::rateEvaluations(){
+
+	m_walkersInPopulation.quickSort(fitnessComparator); // Sort walkers by fitness
+
+	b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness()));
+
+	for(int i = 0; i < m_walkersInPopulation.size();i++){
+		m_walkersInPopulation[i]->setEvaluationTime(0);
+	}
+	m_nextReaped = 0;
 }
 
-btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
-  btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame
-  btVector3 zero(0,0,0);
-  local1.setOrigin(zero);
-  return local1 * axis;
+void NN3DWalkersExample::reap() {
+	int reaped = 0;
+	for(int i = m_walkersInPopulation.size()-1;i >=(m_walkersInPopulation.size()-1)*(1-REAP_QTY); i--){ // reap a certain percentage
+		m_walkersInPopulation[i]->setReaped(true);
+		reaped++;
+		b3Printf("%i Walker(s) reaped.",reaped);
+	}
 }
 
-btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
-  btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame
-  btVector3 zero(0,0,0);
-  local1.setOrigin(zero);
-  return local1 * axis;
+NNWalker* NN3DWalkersExample::getRandomElite(){
+	return m_walkersInPopulation[((m_walkersInPopulation.size()-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)];
 }
 
-btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform) {
-  return localObjectCenterOfMassTransform.inverse() * transform; // transforms the axis from the local frame into the world frame
+NNWalker* NN3DWalkersExample::getRandomNonElite(){
+	return m_walkersInPopulation[(m_walkersInPopulation.size()-1) * SOW_ELITE_QTY + (m_walkersInPopulation.size()-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)];
 }
 
-btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform) {
-  return localObjectCenterOfMassTransform * transform; // transforms the axis from the local frame into the world frame
+NNWalker* NN3DWalkersExample::getNextReaped() {
+	if((m_walkersInPopulation.size()-1) - m_nextReaped >= (m_walkersInPopulation.size()-1) * (1-REAP_QTY)){
+		m_nextReaped++;
+	}
+
+//	if(m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1]->isReaped()){
+		return m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1];
+//	}
+//	else{
+//		return NULL; // we did it wrongly
+//	}
+
+}
+
+void NN3DWalkersExample::sow() {
+	int sow = 0;
+	for(int i = 0; i < m_walkersInPopulation.size() * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures
+		sow++;
+		b3Printf("%i Walker(s) sown.",sow);
+		NNWalker* mother = getRandomElite(); // Get elite partner (father)
+		NNWalker* father = (SOW_ELITE_PARTNER < rand()/RAND_MAX)?getRandomElite():getRandomNonElite(); //Get elite or random partner (mother)
+		NNWalker* offspring = getNextReaped();
+		crossover(mother,father, offspring);
+	}
+
+	for(int i = m_walkersInPopulation.size()*SOW_ELITE_QTY; i < m_walkersInPopulation.size()*(SOW_ELITE_QTY+SOW_MUTATION_QTY);i++){ // create mutants
+		mutate(m_walkersInPopulation[i], MUTATION_RATE / m_walkersInPopulation.size() * SOW_MUTATION_QTY * (i-m_walkersInPopulation.size()*SOW_ELITE_QTY));
+	}
+
+	for(int i = 0; i < m_walkersInPopulation.size() * (REAP_QTY-SOW_CROSSOVER_QTY);i++){
+		sow++;
+		b3Printf("%i Walker(s) sown.",sow);
+		NNWalker* reaped = getNextReaped();
+		reaped->setReaped(false);
+		reaped->randomizeSensoryMotorWeights();
+	}
+}
+
+void NN3DWalkersExample::crossover(NNWalker* mother, NNWalker* father, NNWalker* child) {
+	for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){
+		btScalar random = ((double) rand() / (RAND_MAX));
+
+		if(random >= 0.5f){
+			child->getSensoryMotorWeights()[i] = mother->getSensoryMotorWeights()[i];
+		}
+		else
+		{
+			child->getSensoryMotorWeights()[i] = father->getSensoryMotorWeights()[i];
+		}
+	}
+}
+
+void NN3DWalkersExample::mutate(NNWalker* mutant, float mutationRate) {
+	for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){
+		btScalar random = ((double) rand() / (RAND_MAX));
+
+		if(random >= mutationRate){
+			mutant->getSensoryMotorWeights()[i] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
+		}
+	}
+}
+
+void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep) {
+	NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)world->getWorldUserInfo();
+
+	nnWalkersDemo->update(timeStep);
+}
+
+void NN3DWalkersExample::update(const double timeSinceLastTick) {
+	updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */
+
+	scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */
+}
+
+void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
+	btScalar delta = timeSinceLastTick;
+	float minFPS = 1.f/60.f;
+	if (delta > minFPS){
+		delta = minFPS;
+	}
+
+	m_Time += delta;
+
+	m_targetAccumulator += delta;
+
+	for(int i = 0; i < m_walkersInPopulation.size();i++) // evaluation time passes
+	{
+		if(m_walkersInPopulation[i]->isInEvaluation()){
+			m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time
+			btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
+			char performance[10];
+			sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
+			m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
+		}
+	}
+
+	if(m_targetAccumulator >= 1.0f /((double)m_targetFrequency))
+	{
+		m_targetAccumulator = 0;
+
+		for (int r=0; r<m_walkersInPopulation.size(); r++)
+		{
+			if(m_walkersInPopulation[r]->isInEvaluation())
+			{
+				for (int i = 0; i < 2*NUM_LEGS; i++)
+				{
+					btScalar targetAngle = 0;
+					btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkersInPopulation[r]->getJoints()[i]);
+
+					if(RANDOM_MOVEMENT){
+						targetAngle   = ((double) rand() / (RAND_MAX));
+					}
+					else{ // neural network movement
+
+						// accumulate sensor inputs with weights
+						for(int j = 0; j < JOINT_COUNT;j++){
+							targetAngle += m_walkersInPopulation[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkersInPopulation[r]->getTouchSensor(i);
+						}
+
+						// apply the activation function
+						targetAngle = (tanh(targetAngle)+1.0f)*0.5f;
+					}
+					btScalar targetLimitAngle 	= hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
+					btScalar currentAngle      	= hingeC->getHingeAngle();
+					btScalar angleError  		= targetLimitAngle - currentAngle;
+					btScalar desiredAngularVel 	= angleError/delta;
+					hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength);
+				}
+
+				// clear sensor signals after usage
+				m_walkersInPopulation[r]->clearTouchSensors();
+			}
+		}
+	}
+}
+
+void NN3DWalkersExample::scheduleEvaluations() {
+	for(int i = 0; i < m_walkersInPopulation.size();i++){
+
+		if(m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_TIME){ /**!< tear down evaluations */
+			b3Printf("An evaluation finished at %f s. Distance: %f m", m_Time, btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
+			m_walkersInPopulation[i]->setInEvaluation(false);
+			m_walkersInPopulation[i]->removeFromWorld();
+			m_evaluationsQty--;
+		}
+
+		if(m_evaluationsQty < NUM_PARALLEL_EVALUATIONS && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0){ /**!< Setup the new evaluations */
+			b3Printf("An evaluation started at %f s.",m_Time);
+			m_evaluationsQty++;
+			m_walkersInPopulation[i]->setInEvaluation(true);
+			m_walkersInPopulation[i]->resetAt(btVector3(0,0,0));
+			m_walkersInPopulation[i]->addToWorld();
+			m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+		}
+	}
+
+	if(m_evaluationsQty == 0){ // if there are no more evaluations possible
+		rateEvaluations(); // rate evaluations by sorting them based on their fitness
+
+		reap(); // reap worst performing walkers
+
+		sow(); // crossover & mutate and sow new walkers
+		b3Printf("### A new generation started. ###");
+	}
 }
diff --git a/examples/Evolution/NN3DWalkers.h b/examples/Evolution/NN3DWalkers.h
index 0e6f0c7e6..59105f94c 100755
--- a/examples/Evolution/NN3DWalkers.h
+++ b/examples/Evolution/NN3DWalkers.h
@@ -1,6 +1,6 @@
 /*
-Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
-Motor Demo
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
diff --git a/examples/Utils/b3ReferenceFrameHelper.hpp b/examples/Utils/b3ReferenceFrameHelper.hpp
new file mode 100755
index 000000000..06f63f087
--- /dev/null
+++ b/examples/Utils/b3ReferenceFrameHelper.hpp
@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_REFERENCEFRAMEHELPER_H
+#define B3_REFERENCEFRAMEHELPER_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+
+class b3ReferenceFrameHelper {
+public:
+	static btVector3 getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 point) {
+		  return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame
+	}
+
+	static btVector3 getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 point) {
+		  return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame
+	}
+
+	static btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
+	  btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame
+	  btVector3 zero(0,0,0);
+	  local1.setOrigin(zero);
+	  return local1 * axis;
+	}
+
+	static btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
+	  btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame
+	  btVector3 zero(0,0,0);
+	  local1.setOrigin(zero);
+	  return local1 * axis;
+	}
+
+	static btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+	  return localObjectCenterOfMassTransform.inverse() * transform; // transforms the axis from the local frame into the world frame
+	}
+
+	static btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+	  return localObjectCenterOfMassTransform * transform; // transforms the axis from the local frame into the world frame
+	}
+
+};
+
+#endif /* B3_REFERENCEFRAMEHELPER_H */

From 229d2501af2cf8530f4f10a5550be4ad77a07cde Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Mon, 12 Sep 2016 16:36:37 +0200
Subject: [PATCH 13/22] Add 2m ground circles. Make population config
 printable. Add timeseries plot of individual fitnesses of each generation.

---
 examples/Evolution/NN3DWalkers.cpp | 87 +++++++++++++++++++++++++-----
 1 file changed, 75 insertions(+), 12 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 4110f0d32..78949e7da 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -33,6 +33,7 @@ class NNWalker;
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
 #include "../Utils/b3ReferenceFrameHelper.hpp"
+#include "../RenderingExamples/TimeSeriesCanvas.h"
 
 static btScalar gRootBodyRadius  = 0.25f;
 static btScalar gRootBodyHeight = 0.1f;
@@ -111,9 +112,11 @@ class NN3DWalkersExample : public CommonRigidBodyBase
 	
 	btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;
 	
+	TimeSeriesCanvas* m_timeSeriesCanvas;
+
 public:
 	NN3DWalkersExample(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0),m_evaluationsQty(0),m_nextReaped(0)
+	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0),m_evaluationsQty(0),m_nextReaped(0),m_timeSeriesCanvas(0)
 	{
 	}
 
@@ -152,6 +155,8 @@ public:
 
 	void scheduleEvaluations();
 
+	void drawMarkings();
+
 	// Reaper
 
 	void rateEvaluations();
@@ -170,6 +175,8 @@ public:
 
 	NNWalker* getNextReaped();
 
+	void printWalkerConfigs();
+
 };
 
 static NN3DWalkersExample* nn3DWalkers = NULL;
@@ -639,6 +646,12 @@ void NN3DWalkersExample::initPhysics()
 		btVector3 offset(0,0,0);
 		spawnWalker(offset, false);
 	}
+
+	m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,300,200, "Fitness Performance");
+	m_timeSeriesCanvas ->setupTimeSeries(40, NUM_WALKERS*EVALUATION_TIME, 0);
+	for(int i = 0; i < NUM_WALKERS ; i++){
+		m_timeSeriesCanvas->addDataSource(" ", 100*i/NUM_WALKERS,100*(NUM_WALKERS-i)/NUM_WALKERS,100*(i)/NUM_WALKERS);
+	}
 }
 
 
@@ -704,6 +717,9 @@ bool NN3DWalkersExample::keyboardCallback(int key, int state)
 		m_motorStrength *= 1.1f;
 		return true;
 		break;
+	case 'l':
+		printWalkerConfigs();
+		break;
 	default:
 		break;
 	}
@@ -744,7 +760,7 @@ class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptio
 
 bool fitnessComparator (const NNWalker* a, const NNWalker* b)
 {
-    return a->getFitness() > b->getFitness();
+    return a->getFitness() > b->getFitness(); // sort walkers descending
 }
 
 void NN3DWalkersExample::rateEvaluations(){
@@ -753,6 +769,11 @@ void NN3DWalkersExample::rateEvaluations(){
 
 	b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness()));
 
+	for(int i = 0; i < m_walkersInPopulation.size();i++){
+		m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),0,true);
+	}
+	m_timeSeriesCanvas->nextTick();
+
 	for(int i = 0; i < m_walkersInPopulation.size();i++){
 		m_walkersInPopulation[i]->setEvaluationTime(0);
 	}
@@ -781,12 +802,12 @@ NNWalker* NN3DWalkersExample::getNextReaped() {
 		m_nextReaped++;
 	}
 
-//	if(m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1]->isReaped()){
+	if(m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1]->isReaped()){
 		return m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1];
-//	}
-//	else{
-//		return NULL; // we did it wrongly
-//	}
+	}
+	else{
+		return NULL; // we asked for too many
+	}
 
 }
 
@@ -805,7 +826,7 @@ void NN3DWalkersExample::sow() {
 		mutate(m_walkersInPopulation[i], MUTATION_RATE / m_walkersInPopulation.size() * SOW_MUTATION_QTY * (i-m_walkersInPopulation.size()*SOW_ELITE_QTY));
 	}
 
-	for(int i = 0; i < m_walkersInPopulation.size() * (REAP_QTY-SOW_CROSSOVER_QTY);i++){
+	for(int i = 0; i < (m_walkersInPopulation.size()-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){
 		sow++;
 		b3Printf("%i Walker(s) sown.",sow);
 		NNWalker* reaped = getNextReaped();
@@ -848,6 +869,8 @@ void NN3DWalkersExample::update(const double timeSinceLastTick) {
 	updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */
 
 	scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */
+
+	drawMarkings();
 }
 
 void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
@@ -865,10 +888,6 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 	{
 		if(m_walkersInPopulation[i]->isInEvaluation()){
 			m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time
-			btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
-			char performance[10];
-			sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
-			m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
 		}
 	}
 
@@ -941,3 +960,47 @@ void NN3DWalkersExample::scheduleEvaluations() {
 		b3Printf("### A new generation started. ###");
 	}
 }
+
+void NN3DWalkersExample::drawMarkings() {
+	for(int i = 0; i < m_walkersInPopulation.size();i++) // draw current distance plates of moving walkers
+	{
+		if(m_walkersInPopulation[i]->isInEvaluation()){
+			btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
+			char performance[10];
+			sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
+			m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
+		}
+	}
+
+	for(int i = 2; i < 50; i+=2){ // draw distance circles
+		if(m_dynamicsWorld->getDebugDrawer()){
+			m_dynamicsWorld->getDebugDrawer()->drawArc(btVector3(0,0,0),btVector3(0,1,0),btVector3(1,0,0),btScalar(i), btScalar(i),btScalar(0),btScalar(SIMD_2_PI),btVector3(10*i,0,0),false);
+		}
+	}
+}
+
+void NN3DWalkersExample::printWalkerConfigs(){
+	char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
+	char* runner = configString;
+	sprintf(runner,"Population configuration:");
+	runner +=25;
+	for(int i = 0;i < NUM_WALKERS;i++) {
+		runner[0] = '\n';
+		runner++;
+		runner[0] = '[';
+		runner++;
+		for(int j = 0; j < BODYPART_COUNT*JOINT_COUNT;j++) {
+			sprintf(runner,"%.15f", m_walkersInPopulation[i]->getSensoryMotorWeights()[j]);
+			runner +=15;
+			if(j + 1 != BODYPART_COUNT*JOINT_COUNT){
+				runner[0] = ',';
+			}
+			else{
+				runner[0] = ']';
+			}
+			runner++;
+		}
+	}
+	runner[0] = '\0';
+	b3Printf(configString);
+}

From c0285bc4254f61ed8badac5425d038275a014587 Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Mon, 12 Sep 2016 19:42:07 +0200
Subject: [PATCH 14/22] Add slider to control number of parallel evaluations.

---
 examples/Evolution/NN3DWalkers.cpp | 72 ++++++++++++++++++++++++------
 1 file changed, 58 insertions(+), 14 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 78949e7da..0561ea648 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -42,6 +42,8 @@ static btScalar gLegLength = 0.45f;
 static btScalar gForeLegLength = 0.75f;
 static btScalar gForeLegRadius = 0.08f;
 
+static btScalar gParallelEvaluations = 10.0f;
+
 #ifndef SIMD_PI_4
 #define SIMD_PI_4     0.5 * SIMD_HALF_PI
 #endif
@@ -62,10 +64,6 @@ static btScalar gForeLegRadius = 0.08f;
 #define NUM_WALKERS 50
 #endif
 
-#ifndef NUM_PARALLEL_EVALUATIONS
-#define NUM_PARALLEL_EVALUATIONS 1
-#endif
-
 #ifndef EVALUATION_TIME
 #define EVALUATION_TIME 10 // s
 #endif
@@ -101,6 +99,8 @@ static btScalar gForeLegRadius = 0.08f;
 
 void* GROUND_ID = (void*)1;
 
+//TODO: Fix bug that happens randomly and lets creatures not be visible
+
 class NN3DWalkersExample : public CommonRigidBodyBase
 {
 	btScalar m_Time;
@@ -128,7 +128,7 @@ public:
 	
 	virtual void exitPhysics();
 	
-	void spawnWalker(const btVector3& startOffset, bool bFixed);
+	void spawnWalker(int index, const btVector3& startOffset, bool bFixed);
 	
 	virtual bool	keyboardCallback(int key, int state);
 	
@@ -196,6 +196,7 @@ class NNWalker
 	btScalar			m_evaluationTime;
 	bool				m_reaped;
 	btVector3			m_startPosition;
+	int					m_index;
 
 	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
 	{
@@ -224,9 +225,10 @@ public:
 		}
 	}
 
-	NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
+	NNWalker(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
 		: m_ownerWorld (ownerWorld), m_inEvaluation(false), m_evaluationTime(0), m_reaped(false)
 	{
+		m_index = index;
 		btVector3 vUp(0, 1, 0); // up in local reference frame
 
 		NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();
@@ -492,6 +494,10 @@ public:
 	void setReaped(bool reaped) {
 		m_reaped = reaped;
 	}
+
+	int getIndex() const {
+		return m_index;
+	}
 };
 
 void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep);
@@ -504,24 +510,45 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
     void* ID1 = o1->getUserPointer();
     void* ID2 = o2->getUserPointer();
 
-	if (ID2 != GROUND_ID || ID1 != GROUND_ID) {
+	if (ID1 != GROUND_ID || ID2 != GROUND_ID) {
 	    // Make a circle with a 0.9 radius at (0,0,0)
 	    // with RGB color (1,0,0).
 		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL){
 			nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
 		}
 
-		if(ID1 != GROUND_ID){
+		if(ID1 != GROUND_ID && ID1){
 			((NNWalker*)ID1)->setTouchSensor(o1);
 		}
 
-		if(ID2 != GROUND_ID){
+		if(ID2 != GROUND_ID && ID2){
 			((NNWalker*)ID2)->setTouchSensor(o2);
 		}
 	}
     return false;
 }
 
+struct WalkerFilterCallback : public btOverlapFilterCallback
+{
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
+	{
+		btCollisionObject* obj0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
+		btCollisionObject* obj1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
+
+		if (obj0->getUserPointer() == GROUND_ID || obj1->getUserPointer() == GROUND_ID) { // everything collides with ground
+			return true;
+		}
+		else{
+			return ((NNWalker*)obj0->getUserPointer())->getIndex() == ((NNWalker*)obj1->getUserPointer())->getIndex();
+		}
+	}
+};
+
+void floorNNSliderValue(float notUsed) {
+	gParallelEvaluations = floor(gParallelEvaluations);
+}
+
 void NN3DWalkersExample::initPhysics()
 {
 	gContactProcessedCallback = legContactProcessedCallback;
@@ -616,6 +643,16 @@ void NN3DWalkersExample::initPhysics()
 			slider);
 	}
 
+	{ // create a slider to change the number of parallel evaluations
+		SliderParams slider("Parallel evaluations", &gParallelEvaluations);
+		slider.m_minVal = 1;
+		slider.m_maxVal = NUM_WALKERS;
+		slider.m_clampToNotches = false;
+		slider.m_callback = floorNNSliderValue; // hack to get integer values
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
+
 
 	// Setup a big ground box
 	{
@@ -644,9 +681,12 @@ void NN3DWalkersExample::initPhysics()
 
 		// Spawn one walker
 		btVector3 offset(0,0,0);
-		spawnWalker(offset, false);
+		spawnWalker(i, offset, false);
 	}
 
+	btOverlapFilterCallback * filterCallback = new WalkerFilterCallback();
+	m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback);
+
 	m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,300,200, "Fitness Performance");
 	m_timeSeriesCanvas ->setupTimeSeries(40, NUM_WALKERS*EVALUATION_TIME, 0);
 	for(int i = 0; i < NUM_WALKERS ; i++){
@@ -655,9 +695,9 @@ void NN3DWalkersExample::initPhysics()
 }
 
 
-void NN3DWalkersExample::spawnWalker(const btVector3& startOffset, bool bFixed)
+void NN3DWalkersExample::spawnWalker(int index, const btVector3& startOffset, bool bFixed)
 {
-	NNWalker* walker = new NNWalker(m_dynamicsWorld, startOffset, bFixed);
+	NNWalker* walker = new NNWalker(index, m_dynamicsWorld, startOffset, bFixed);
 	m_walkersInPopulation.push_back(walker);
 }
 
@@ -941,11 +981,15 @@ void NN3DWalkersExample::scheduleEvaluations() {
 			m_evaluationsQty--;
 		}
 
-		if(m_evaluationsQty < NUM_PARALLEL_EVALUATIONS && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0){ /**!< Setup the new evaluations */
+		if(m_evaluationsQty < gParallelEvaluations && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0){ /**!< Setup the new evaluations */
 			b3Printf("An evaluation started at %f s.",m_Time);
 			m_evaluationsQty++;
 			m_walkersInPopulation[i]->setInEvaluation(true);
-			m_walkersInPopulation[i]->resetAt(btVector3(0,0,0));
+
+			if(m_walkersInPopulation[i]->getEvaluationTime() == 0){ // reset to origin if the evaluation did not yet run
+				m_walkersInPopulation[i]->resetAt(btVector3(0,0,0));
+			}
+
 			m_walkersInPopulation[i]->addToWorld();
 			m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 		}

From faada023be89043fe3db96937554b60824709853 Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Mon, 12 Sep 2016 23:59:42 +0200
Subject: [PATCH 15/22] Fix error happening when using btQuaternion() to create
 Unit quaternion instead of btQuaternion::getIdentity(). Fix problems raised
 by Bullet CI.

---
 examples/Evolution/NN3DWalkers.cpp | 91 +++++++++++++++++-------------
 1 file changed, 51 insertions(+), 40 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 0561ea648..2c14758a5 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -99,8 +99,6 @@ static btScalar gParallelEvaluations = 10.0f;
 
 void* GROUND_ID = (void*)1;
 
-//TODO: Fix bug that happens randomly and lets creatures not be visible
-
 class NN3DWalkersExample : public CommonRigidBodyBase
 {
 	btScalar m_Time;
@@ -116,7 +114,14 @@ class NN3DWalkersExample : public CommonRigidBodyBase
 
 public:
 	NN3DWalkersExample(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper), m_Time(0),m_motorStrength(0.5f),m_targetFrequency(3),m_targetAccumulator(0),m_evaluationsQty(0),m_nextReaped(0),m_timeSeriesCanvas(0)
+	:CommonRigidBodyBase(helper),
+	 m_Time(0),
+	 m_motorStrength(0.5f),
+	 m_targetFrequency(3),
+	 m_targetAccumulator(0),
+	 m_evaluationsQty(0),
+	 m_nextReaped(0),
+	 m_timeSeriesCanvas(0)
 	{
 	}
 
@@ -149,9 +154,9 @@ public:
 
 	// Evaluation
 
-	void update(const double timeSinceLastTick);
+	void update(const btScalar timeSinceLastTick);
 
-	void updateEvaluations(const double timeSinceLastTick);
+	void updateEvaluations(const btScalar timeSinceLastTick);
 
 	void scheduleEvaluations();
 
@@ -167,7 +172,7 @@ public:
 
 	void crossover(NNWalker* mother, NNWalker* father, NNWalker* offspring);
 
-	void mutate(NNWalker* mutant, float mutationRate);
+	void mutate(NNWalker* mutant, btScalar mutationRate);
 
 	NNWalker* getRandomElite();
 
@@ -226,7 +231,10 @@ public:
 	}
 
 	NNWalker(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
-		: m_ownerWorld (ownerWorld), m_inEvaluation(false), m_evaluationTime(0), m_reaped(false)
+		: m_ownerWorld (ownerWorld),
+		  m_inEvaluation(false),
+		  m_evaluationTime(0),
+		  m_reaped(false)
 	{
 		m_index = index;
 		btVector3 vUp(0, 1, 0); // up in local reference frame
@@ -419,7 +427,7 @@ public:
 			m_ownerWorld->removeConstraint(m_joints[i]);
 		}
 
-		// Remove all bodies and shapes
+		// Remove all bodies
 		for ( i = 0; i < BODYPART_COUNT; ++i)
 		{
 			m_ownerWorld->removeRigidBody(m_bodies[i]);
@@ -431,8 +439,6 @@ public:
 
 		for(int i = 0; i < BODYPART_COUNT;i++)
 		{
-			//b3Printf(" position (%f,%f,%f)",m_bodies[i]->getCenterOfMassPosition().x(),m_bodies[i]->getCenterOfMassPosition().y(),m_bodies[i]->getCenterOfMassPosition().z());
-
 			finalPosition += m_bodies[i]->getCenterOfMassPosition();
 		}
 
@@ -455,7 +461,7 @@ public:
 	}
 
 	void resetAt(btVector3 position) {
-		btTransform resetPosition(btQuaternion(), position);
+		btTransform resetPosition(btQuaternion::getIdentity(), position);
 		for (int i = 0; i < BODYPART_COUNT; ++i)
 		{
 			m_bodies[i]->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]);
@@ -471,11 +477,11 @@ public:
 		clearTouchSensors();
 	}
 
-	double getEvaluationTime() const {
+	btScalar getEvaluationTime() const {
 		return m_evaluationTime;
 	}
 
-	void setEvaluationTime(double evaluationTime) {
+	void setEvaluationTime(btScalar evaluationTime) {
 		m_evaluationTime = evaluationTime;
 	}
 
@@ -746,7 +752,6 @@ bool NN3DWalkersExample::detectCollisions()
 
 bool NN3DWalkersExample::keyboardCallback(int key, int state)
 {
-	//TODO: Redesign the button setups
 	switch (key)
 	{
 	case '[':
@@ -774,7 +779,7 @@ void NN3DWalkersExample::exitPhysics()
 
 	int i;
 
-	for (i = 0;i < m_walkersInPopulation.size();i++)
+	for (i = 0;i < NUM_WALKERS;i++)
 	{
 		NNWalker* walker = m_walkersInPopulation[i];
 		delete walker;
@@ -809,12 +814,12 @@ void NN3DWalkersExample::rateEvaluations(){
 
 	b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness()));
 
-	for(int i = 0; i < m_walkersInPopulation.size();i++){
+	for(int i = 0; i < NUM_WALKERS;i++){
 		m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),0,true);
 	}
 	m_timeSeriesCanvas->nextTick();
 
-	for(int i = 0; i < m_walkersInPopulation.size();i++){
+	for(int i = 0; i < NUM_WALKERS;i++){
 		m_walkersInPopulation[i]->setEvaluationTime(0);
 	}
 	m_nextReaped = 0;
@@ -822,7 +827,7 @@ void NN3DWalkersExample::rateEvaluations(){
 
 void NN3DWalkersExample::reap() {
 	int reaped = 0;
-	for(int i = m_walkersInPopulation.size()-1;i >=(m_walkersInPopulation.size()-1)*(1-REAP_QTY); i--){ // reap a certain percentage
+	for(int i = NUM_WALKERS-1;i >=(NUM_WALKERS-1)*(1-REAP_QTY); i--){ // reap a certain percentage
 		m_walkersInPopulation[i]->setReaped(true);
 		reaped++;
 		b3Printf("%i Walker(s) reaped.",reaped);
@@ -830,20 +835,20 @@ void NN3DWalkersExample::reap() {
 }
 
 NNWalker* NN3DWalkersExample::getRandomElite(){
-	return m_walkersInPopulation[((m_walkersInPopulation.size()-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)];
+	return m_walkersInPopulation[((NUM_WALKERS-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)];
 }
 
 NNWalker* NN3DWalkersExample::getRandomNonElite(){
-	return m_walkersInPopulation[(m_walkersInPopulation.size()-1) * SOW_ELITE_QTY + (m_walkersInPopulation.size()-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)];
+	return m_walkersInPopulation[(NUM_WALKERS-1) * SOW_ELITE_QTY + (NUM_WALKERS-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)];
 }
 
 NNWalker* NN3DWalkersExample::getNextReaped() {
-	if((m_walkersInPopulation.size()-1) - m_nextReaped >= (m_walkersInPopulation.size()-1) * (1-REAP_QTY)){
+	if((NUM_WALKERS-1) - m_nextReaped >= (NUM_WALKERS-1) * (1-REAP_QTY)){
 		m_nextReaped++;
 	}
 
-	if(m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1]->isReaped()){
-		return m_walkersInPopulation[(m_walkersInPopulation.size()-1) - m_nextReaped+1];
+	if(m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1]->isReaped()){
+		return m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1];
 	}
 	else{
 		return NULL; // we asked for too many
@@ -853,20 +858,20 @@ NNWalker* NN3DWalkersExample::getNextReaped() {
 
 void NN3DWalkersExample::sow() {
 	int sow = 0;
-	for(int i = 0; i < m_walkersInPopulation.size() * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures
+	for(int i = 0; i < NUM_WALKERS * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures
 		sow++;
 		b3Printf("%i Walker(s) sown.",sow);
-		NNWalker* mother = getRandomElite(); // Get elite partner (father)
-		NNWalker* father = (SOW_ELITE_PARTNER < rand()/RAND_MAX)?getRandomElite():getRandomNonElite(); //Get elite or random partner (mother)
+		NNWalker* mother = getRandomElite(); // Get elite partner (mother)
+		NNWalker* father = (SOW_ELITE_PARTNER < rand()/RAND_MAX)?getRandomElite():getRandomNonElite(); //Get elite or random partner (father)
 		NNWalker* offspring = getNextReaped();
 		crossover(mother,father, offspring);
 	}
 
-	for(int i = m_walkersInPopulation.size()*SOW_ELITE_QTY; i < m_walkersInPopulation.size()*(SOW_ELITE_QTY+SOW_MUTATION_QTY);i++){ // create mutants
-		mutate(m_walkersInPopulation[i], MUTATION_RATE / m_walkersInPopulation.size() * SOW_MUTATION_QTY * (i-m_walkersInPopulation.size()*SOW_ELITE_QTY));
+	for(int i = NUM_WALKERS*SOW_ELITE_QTY; i < NUM_WALKERS*(SOW_ELITE_QTY+SOW_MUTATION_QTY);i++){ // create mutants
+		mutate(m_walkersInPopulation[i], btScalar(MUTATION_RATE / (NUM_WALKERS * SOW_MUTATION_QTY) * (i-NUM_WALKERS*SOW_ELITE_QTY)));
 	}
 
-	for(int i = 0; i < (m_walkersInPopulation.size()-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){
+	for(int i = 0; i < (NUM_WALKERS-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){
 		sow++;
 		b3Printf("%i Walker(s) sown.",sow);
 		NNWalker* reaped = getNextReaped();
@@ -889,7 +894,7 @@ void NN3DWalkersExample::crossover(NNWalker* mother, NNWalker* father, NNWalker*
 	}
 }
 
-void NN3DWalkersExample::mutate(NNWalker* mutant, float mutationRate) {
+void NN3DWalkersExample::mutate(NNWalker* mutant, btScalar mutationRate) {
 	for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){
 		btScalar random = ((double) rand() / (RAND_MAX));
 
@@ -905,17 +910,17 @@ void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep)
 	nnWalkersDemo->update(timeStep);
 }
 
-void NN3DWalkersExample::update(const double timeSinceLastTick) {
+void NN3DWalkersExample::update(const btScalar timeSinceLastTick) {
 	updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */
 
 	scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */
 
-	drawMarkings();
+	drawMarkings(); /**!< Draw markings on the ground */
 }
 
 void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 	btScalar delta = timeSinceLastTick;
-	float minFPS = 1.f/60.f;
+	btScalar minFPS = 1.f/60.f;
 	if (delta > minFPS){
 		delta = minFPS;
 	}
@@ -924,7 +929,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 
 	m_targetAccumulator += delta;
 
-	for(int i = 0; i < m_walkersInPopulation.size();i++) // evaluation time passes
+	for(int i = 0; i < NUM_WALKERS;i++) // evaluation time passes
 	{
 		if(m_walkersInPopulation[i]->isInEvaluation()){
 			m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time
@@ -935,7 +940,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 	{
 		m_targetAccumulator = 0;
 
-		for (int r=0; r<m_walkersInPopulation.size(); r++)
+		for (int r=0; r<NUM_WALKERS; r++)
 		{
 			if(m_walkersInPopulation[r]->isInEvaluation())
 			{
@@ -960,7 +965,13 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 					btScalar targetLimitAngle 	= hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
 					btScalar currentAngle      	= hingeC->getHingeAngle();
 					btScalar angleError  		= targetLimitAngle - currentAngle;
-					btScalar desiredAngularVel 	= angleError/delta;
+					btScalar desiredAngularVel = 0;
+					if(delta){
+						desiredAngularVel = angleError/delta;
+					}
+					else{
+						desiredAngularVel = angleError/0.0001f;
+					}
 					hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength);
 				}
 
@@ -972,7 +983,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 }
 
 void NN3DWalkersExample::scheduleEvaluations() {
-	for(int i = 0; i < m_walkersInPopulation.size();i++){
+	for(int i = 0; i < NUM_WALKERS;i++){
 
 		if(m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_TIME){ /**!< tear down evaluations */
 			b3Printf("An evaluation finished at %f s. Distance: %f m", m_Time, btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
@@ -1006,7 +1017,7 @@ void NN3DWalkersExample::scheduleEvaluations() {
 }
 
 void NN3DWalkersExample::drawMarkings() {
-	for(int i = 0; i < m_walkersInPopulation.size();i++) // draw current distance plates of moving walkers
+	for(int i = 0; i < NUM_WALKERS;i++) // draw current distance plates of moving walkers
 	{
 		if(m_walkersInPopulation[i]->isInEvaluation()){
 			btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
@@ -1024,7 +1035,7 @@ void NN3DWalkersExample::drawMarkings() {
 }
 
 void NN3DWalkersExample::printWalkerConfigs(){
-	char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
+	char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
 	char* runner = configString;
 	sprintf(runner,"Population configuration:");
 	runner +=25;
@@ -1036,7 +1047,7 @@ void NN3DWalkersExample::printWalkerConfigs(){
 		for(int j = 0; j < BODYPART_COUNT*JOINT_COUNT;j++) {
 			sprintf(runner,"%.15f", m_walkersInPopulation[i]->getSensoryMotorWeights()[j]);
 			runner +=15;
-			if(j + 1 != BODYPART_COUNT*JOINT_COUNT){
+			if(j + 1 < BODYPART_COUNT*JOINT_COUNT){
 				runner[0] = ',';
 			}
 			else{

From f0f694145ddce1300c8e507326d9d7a15c1b070d Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Fri, 16 Sep 2016 09:49:18 +0200
Subject: [PATCH 16/22] Implement TimeWarpBase.

-------------------------------------
This commit implements speeding up and slowing down examples. The
example can be influenced by the parameters. A separate example will be
added to show off the capabilities of the TimeWarpBase for other
examples.

The walkers work quite well, a successful evolution was run over night
and reached a walker distance of 7.2m.
---
 .../CommonInterfaces/CommonTimeWarpBase.h     | 829 ++++++++++++++++++
 examples/Evolution/NN3DWalkers.cpp            |  27 +-
 examples/ExampleBrowser/CMakeLists.txt        |   4 +-
 examples/Utils/b3ERPCFMHelper.hpp             |  79 ++
 4 files changed, 926 insertions(+), 13 deletions(-)
 create mode 100755 examples/CommonInterfaces/CommonTimeWarpBase.h
 create mode 100755 examples/Utils/b3ERPCFMHelper.hpp

diff --git a/examples/CommonInterfaces/CommonTimeWarpBase.h b/examples/CommonInterfaces/CommonTimeWarpBase.h
new file mode 100755
index 000000000..371b79997
--- /dev/null
+++ b/examples/CommonInterfaces/CommonTimeWarpBase.h
@@ -0,0 +1,829 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+
+ 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.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef COMMON_TIME_WARP_BASE_H
+#define COMMON_TIME_WARP_BASE_H
+
+#include <string>
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btQuickprof.h" // Use your own timer, this timer is only used as we lack another timer
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
+#include <BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h>
+#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
+#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
+#include <BulletDynamics/MLCPSolvers/btDantzigSolver.h>
+#include <BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h>
+#include <BulletDynamics/MLCPSolvers/btLemkeSolver.h>
+#include <BulletDynamics/MLCPSolvers/btMLCPSolver.h>
+
+#include "../Utils/b3ERPCFMHelper.hpp" // ERP/CFM setting utils
+
+static btScalar gSimulationSpeed = 1; // default simulation speed at startup
+
+// the current simulation speeds to choose from (the slider will snap to those using a custom form of snapping)
+namespace SimulationSpeeds {
+static double/*0*/PAUSE = 0;
+static double/*1*/QUARTER_SPEED = 0.25;
+static double/*2*/HALF_SPEED = 0.5;
+static double/*3*/NORMAL_SPEED = 1;
+static double/*4*/DOUBLE_SPEED = 2;
+static double/*5*/QUADRUPLE_SPEED = 4;
+static double/*6*/DECUPLE_SPEED = 10;
+static double/*7*/CENTUPLE_SPEED = 100;
+static double/*8*/QUINCENTUPLE_SPEED = 500;
+static double /*9*/ MILLITUPLE_SPEED = 1000;
+static double/*0*/MAX_SPEED = MILLITUPLE_SPEED;
+static double /**/NUM_SPEEDS = 11;
+};
+
+// add speeds from the namespace here
+static double speeds[] = { SimulationSpeeds::PAUSE,
+		SimulationSpeeds::QUARTER_SPEED, SimulationSpeeds::HALF_SPEED,
+		SimulationSpeeds::NORMAL_SPEED, SimulationSpeeds::DOUBLE_SPEED,
+		SimulationSpeeds::QUADRUPLE_SPEED, SimulationSpeeds::DECUPLE_SPEED,
+		SimulationSpeeds::CENTUPLE_SPEED,SimulationSpeeds::QUINCENTUPLE_SPEED,
+		SimulationSpeeds::MILLITUPLE_SPEED};
+
+static btScalar gSolverIterations = 10; // default number of solver iterations for the iterative solvers
+
+static bool gChangeErpCfm = false;
+
+static bool gIsHeadless = false; // demo runs with graphics by default
+
+static int gMinSpeed = SimulationSpeeds::PAUSE; // the minimum simulation speed
+
+static int gMaxSpeed = SimulationSpeeds::MAX_SPEED; // the maximum simulation speed
+
+static bool gMaximumSpeed = false; // the demo does not try to achieve maximum stepping speed by default
+
+static bool gInterpolate = false; // the demo does not use any bullet interpolated physics substeps
+
+static bool useSplitImpulse = true; // split impulse fixes issues with restitution in Baumgarte stabilization
+// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=7117&p=24631&hilit=Baumgarte#p24631
+// disabling continuous collision detection can also fix issues with restitution, though CCD is disabled by default an only kicks in at higher speeds
+// set CCD speed threshold and testing sphere radius per rigidbody (rb->setCCDSpeedThreshold())
+
+// all supported solvers by bullet
+enum SolverEnumType {
+	SEQUENTIALIMPULSESOLVER = 0,
+	GAUSSSEIDELSOLVER = 1,
+	NNCGSOLVER = 2,
+	DANZIGSOLVER = 3,
+	LEMKESOLVER = 4,
+	FSSOLVER = 5,
+	NUM_SOLVERS = 6
+};
+
+
+//TODO: In case the solver should be changeable by drop down menu
+namespace SolverType {
+static char SEQUENTIALIMPULSESOLVER[] = "Sequential Impulse Solver";
+static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver";
+static char NNCGSOLVER[] = "NNCG Solver";
+static char DANZIGSOLVER[] = "Danzig Solver";
+static char LEMKESOLVER[] = "Lemke Solver";
+static char FSSOLVER[] = "FeatherStone Solver";
+};
+
+static const char* solverTypes[NUM_SOLVERS];
+
+static SolverEnumType SOLVER_TYPE = SEQUENTIALIMPULSESOLVER; // You can switch the solver here
+
+//TODO:s===
+//TODO: Give specific explanations about solver values
+
+/**
+ * Step size of the bullet physics simulator (solverAccuracy). Accuracy versus speed.
+ */
+// Choose an appropriate number of steps per second for your needs
+static btScalar gPhysicsStepsPerSecond = 60.0f; // Default number of steps
+//static btScalar gPhysicsStepsPerSecond = 120.0f; // Double steps for more accuracy
+//static btScalar gPhysicsStepsPerSecond = 240.0f; // For high accuracy
+//static btScalar gPhysicsStepsPerSecond = 1000.0f; // Very high accuracy
+
+// appropriate inverses for seconds and milliseconds
+static double fixedPhysicsStepSizeSec = 1.0f / gPhysicsStepsPerSecond; // steps size in seconds
+static double fixedPhysicsStepSizeMilli = 1000.0f / gPhysicsStepsPerSecond; // step size in milliseconds
+
+static btScalar gApplicationFrequency = 120.0f; // number of internal application ticks per second
+static int gApplicationTick = 1000.0f / gApplicationFrequency; //ms
+
+static btScalar gFramesPerSecond = 30.0f; // number of frames per second
+
+static btScalar gERPSpringK = 10;
+static btScalar gERPDamperC = 1;
+
+static btScalar gCFMSpringK = 10;
+static btScalar gCFMDamperC = 1;
+static btScalar gCFMSingularityAvoidance = 0;
+
+//GUI related parameter changing helpers
+
+inline void floorSliderValues(float notUsed) { // floor values that should be ints
+	gSolverIterations = floor(gSolverIterations);
+}
+
+inline void twxChangePhysicsStepsPerSecond(float physicsStepsPerSecond) { // function to change simulation physics steps per second
+	gPhysicsStepsPerSecond = physicsStepsPerSecond;
+}
+
+inline void twxChangeFPS(float framesPerSecond) {
+	gFramesPerSecond = framesPerSecond;
+}
+
+inline void twxChangeERPCFM(float notUsed) { // function to change ERP/CFM appropriately
+	gChangeErpCfm = true;
+}
+
+inline void changeSolver(int comboboxId, const char* item, void* userPointer) { // function to change the solver
+	for(int i = 0; i < NUM_SOLVERS;i++){
+		if(strcmp(solverTypes[i], item) == 0){ // if the strings are equal
+			SOLVER_TYPE = ((SolverEnumType)i);
+			//b3Printf("=%s=\n Reset the simulation by double clicking it in the menu list.",item);
+			return;
+		}
+	}
+	//b3Printf("No Change");
+}
+
+
+inline void twxChangeSolverIterations(float notUsed){ // change the solver iterations
+
+	floorSliderValues(0); // floor the values set by slider
+
+}
+
+inline void clampToCustomSpeedNotches(float speed) { // function to clamp to custom speed notches
+	double minSpeed = 0;
+	double minSpeedDist = SimulationSpeeds::MAX_SPEED;
+	for (int i = 0; i < SimulationSpeeds::NUM_SPEEDS; i++) {
+		double speedDist = (speeds[i]-speed >= 0)?speeds[i]-speed:speed-speeds[i]; // float absolute
+
+		if (minSpeedDist > speedDist) {
+			minSpeedDist = speedDist;
+			minSpeed = speeds[i];
+		}
+	}
+	gSimulationSpeed = minSpeed;
+}
+
+inline void switchInterpolated(int buttonId, bool buttonState, void* userPointer){ // toggle if interpolation steps are taken
+	gInterpolate=!gInterpolate;
+	//b3Printf("Interpolate substeps %s", gInterpolate?"on":"off");
+}
+
+inline void switchHeadless(int buttonId, bool buttonState, void* userPointer){ // toggle if the demo should run headless
+	gIsHeadless=!gIsHeadless;
+	//b3Printf("Run headless %s", gIsHeadless?"on":"off");
+}
+
+inline void switchMaximumSpeed(int buttonId, bool buttonState, void* userPointer){ // toggle it the demo should run as fast as possible
+	gMaximumSpeed=!gMaximumSpeed;
+	//b3Printf("Run maximum speed %s", gMaximumSpeed?"on":"off");
+}
+
+inline void setApplicationTick(float frequency){ // set internal application tick
+	gApplicationTick = 1000.0f/frequency;
+}
+
+/**
+ * @link: Gaffer on Games - Fix your timestep: http://gafferongames.com/game-physics/fix-your-timestep/
+ */
+struct CommonTimeWarpBase: public CommonRigidBodyBase {
+
+	CommonTimeWarpBase(struct GUIHelperInterface* helper):
+		CommonRigidBodyBase(helper),
+		mPhysicsStepsPerSecondUpdated(false),
+		mFramesPerSecondUpdated(false),
+		mSolverIterationsUpdated(false) {
+
+		 // main frame timer initialization
+		 mApplicationStart = mLoopTimer.getTimeMilliseconds(); /**!< Initialize when the application started running */
+		 mInputClock = mApplicationStart; /**!< Initialize the last time the input was updated */
+		 mPreviousModelIteration = mApplicationStart;
+		 mThisModelIteration = mApplicationStart;
+		 mApplicationRuntime = mThisModelIteration - mApplicationStart; /**!< Initialize the application runtime */
+
+		 // sub frame time initializations
+		 mGraphicsStart = mApplicationStart; /** !< Initialize the last graphics start */
+		 mModelStart = mApplicationStart; /** !< Initialize the last model start */
+		 mInputStart = mApplicationStart; /** !< Initialize the last input start */
+
+		 mPhysicsStepStart = mApplicationStart; /**!< Initialize the physics step start */
+		 mPhysicsStepEnd = mApplicationStart; /**!< Initialize the physics step end */
+
+		 //durations
+		 mLastGraphicsTick = 0;
+		 mLastModelTick = 0;
+		 mLastInputTick = 0;
+		 mPhysicsTick = 0;
+
+		 mInputDt = 0;
+		 mModelAccumulator = 0;
+		 mFrameTime = 0;
+
+		 fpsTimeStamp = mLoopTimer.getTimeMilliseconds(); // to time the fps
+		 fpsStep = 1000.0f/gFramesPerSecond;
+
+		 // performance measurements for this demo
+		 performanceTimestamp = 0;
+		 performedTime = 0; // time the physics steps consumed
+		 speedUpPrintTimeStamp = mLoopTimer.getTimeSeconds(); // timer to print the speed up periodically
+		 mLoopTimer.reset();
+	 }
+
+	~CommonTimeWarpBase(){
+
+	}
+
+
+	void initPhysics(){ // initialize the demo
+
+		setupParameterInterface(); // setup adjustable sliders and buttons for parameters
+
+		m_guiHelper->setUpAxis(1); // Set Y axis as Up axis
+
+		createEmptyDynamicsWorld(); // create an empty dynamic world
+
+		m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	}
+
+	void setupParameterInterface(){ // setup the adjustable sliders and button for parameters
+
+		solverTypes[0] = SolverType::SEQUENTIALIMPULSESOLVER;
+		solverTypes[1] = SolverType::GAUSSSEIDELSOLVER;
+		solverTypes[2] = SolverType::NNCGSOLVER;
+		solverTypes[3] = SolverType::DANZIGSOLVER;
+		solverTypes[4] = SolverType::LEMKESOLVER;
+		solverTypes[5] = SolverType::FSSOLVER;
+
+		{
+			ComboBoxParams comboParams;
+			comboParams.m_comboboxId = 0;
+			comboParams.m_numItems = NUM_SOLVERS;
+			comboParams.m_startItem = SOLVER_TYPE;
+			comboParams.m_callback = changeSolver;
+
+			comboParams.m_items=solverTypes;
+			m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+		}
+
+		{ // create a slider to adjust the simulation speed
+			// Force increase the simulation speed to run the simulation with the same accuracy but a higher speed
+			SliderParams slider("Simulation speed",
+				&gSimulationSpeed);
+			slider.m_minVal = gMinSpeed;
+			slider.m_maxVal = gMaxSpeed;
+			slider.m_callback = clampToCustomSpeedNotches;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a button to switch to headless simulation
+			// This turns off the graphics update and therefore results in more time for the model update
+			ButtonParams button("Run headless",0,true);
+			button.m_callback = switchHeadless;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerButtonParameter(
+					button);
+		}
+
+		{ // create a slider to adjust the number of internal application ticks
+			// The set application tick should contain enough time to perform a full cycle of model update (physics and input)
+			// and view update (graphics) with average application load. The graphics and input update determine the remaining time
+			// for the physics update
+			SliderParams slider("Application Ticks",
+				&gApplicationFrequency);
+			slider.m_minVal = gMinSpeed;
+			slider.m_maxVal = gMaxSpeed;
+			slider.m_callback = setApplicationTick;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a button to switch to maximum speed simulation (fully deterministic)
+			// Interesting to test the maximal achievable speed on this hardware
+			ButtonParams button("Run maximum speed",0,true);
+			button.m_callback = switchMaximumSpeed;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerButtonParameter(
+					button);
+		}
+
+		{ // create a slider to adjust the number of physics steps per second
+			// The default number of steps is at 60, which is appropriate for most general simulations
+			// For simulations with higher complexity or if you experience undesired behavior, try increasing the number of steps per second
+			// Alternatively, try increasing the number of solver iterations if you experience jittering constraints due to non-converging solutions
+			SliderParams slider("Physics steps per second", &gPhysicsStepsPerSecond);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1000;
+			slider.m_callback = twxChangePhysicsStepsPerSecond;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a slider to adjust the number of frames per second
+			SliderParams slider("Frames per second", &gFramesPerSecond);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 200;
+			slider.m_callback = twxChangeFPS;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a button to switch bullet to perform interpolated substeps to speed up simulation
+			// generally, interpolated steps are a good speed-up and should only be avoided if higher accuracy is needed (research purposes etc.)
+			ButtonParams button("Perform interpolated substeps",0,true);
+			button.m_callback = switchInterpolated;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerButtonParameter(
+					button);
+		}
+
+		{ // create a slider to adjust the number of solver iterations to converge to a solution
+			// more complex simulations might need a higher number of iterations to converge, it also
+			// depends on the type of solver.
+			SliderParams slider(
+				"Solver interations",
+				&gSolverIterations);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 1000;
+			slider.m_callback = twxChangePhysicsStepsPerSecond;
+			slider.m_clampToNotches = false;
+			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+				slider);
+		}
+
+		// ERP/CFM sliders
+		// Advanced users: Check descriptions of ERP/CFM in BulletUtils.cpp
+
+		{ // create a slider to adjust ERP Spring k constant
+			SliderParams slider("Global ERP Spring k (F=k*x)", &gERPSpringK);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 10;
+			slider.m_callback = twxChangeERPCFM;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a slider to adjust ERP damper c constant
+			SliderParams slider("Global ERP damper c (F=c*xdot)", &gERPDamperC);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 10;
+			slider.m_callback = twxChangeERPCFM;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a slider to adjust CFM Spring k constant
+			SliderParams slider("Global CFM Spring k (F=k*x)", &gCFMSpringK);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 10;
+			slider.m_callback = twxChangeERPCFM;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a slider to adjust CFM damper c constant
+			SliderParams slider("Global CFM damper c (F=c*xdot)", &gCFMDamperC);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 10;
+			slider.m_callback = twxChangeERPCFM;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+
+		{ // create a slider to adjust CFM damper c constant
+			SliderParams slider("Global CFM singularity avoidance", &gCFMSingularityAvoidance);
+			slider.m_minVal = 0;
+			slider.m_maxVal = 10;
+			slider.m_callback = twxChangeERPCFM;
+			slider.m_clampToNotches = false;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+					slider);
+		}
+	}
+
+	void createEmptyDynamicsWorld(){ // create an empty dynamics worlds according to the chosen settings via statics (top section of code)
+
+		///collision configuration contains default setup for memory, collision setup
+		m_collisionConfiguration = new btDefaultCollisionConfiguration();
+		//m_collisionConfiguration->setConvexConvexMultipointIterations();
+
+		///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+		m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+		// default broadphase
+		m_broadphase = new btDbvtBroadphase();
+
+		// different solvers require different settings
+		switch (SOLVER_TYPE) {
+		case SEQUENTIALIMPULSESOLVER: {
+			//b3Printf("=%s=",SolverType::SEQUENTIALIMPULSESOLVER);
+			m_solver = new btSequentialImpulseConstraintSolver();
+			break;
+		}
+		case NNCGSOLVER: {
+			//b3Printf("=%s=",SolverType::NNCGSOLVER);
+			m_solver = new btNNCGConstraintSolver();
+			break;
+		}
+		case DANZIGSOLVER: {
+			//b3Printf("=%s=",SolverType::DANZIGSOLVER);
+			btDantzigSolver* mlcp = new btDantzigSolver();
+			m_solver = new btMLCPSolver(mlcp);
+			break;
+		}
+		case GAUSSSEIDELSOLVER: {
+			//b3Printf("=%s=",SolverType::GAUSSSEIDELSOLVER);
+			btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
+			m_solver = new btMLCPSolver(mlcp);
+			break;
+		}
+		case LEMKESOLVER: {
+			//b3Printf("=%s=",SolverType::LEMKESOLVER);
+			btLemkeSolver* mlcp = new btLemkeSolver();
+			m_solver = new btMLCPSolver(mlcp);
+			break;
+		}
+		case FSSOLVER: {
+			//b3Printf("=%s=",SolverType::FSSOLVER);
+			//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
+			m_solver = new btMultiBodyConstraintSolver;
+
+			break;
+		}
+		default:
+			break;
+		}
+
+		if (SOLVER_TYPE != FSSOLVER) {
+			//TODO: Set parameters for other solvers
+
+			m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,
+				m_broadphase, m_solver, m_collisionConfiguration);
+
+			if (SOLVER_TYPE == DANZIGSOLVER || SOLVER_TYPE == GAUSSSEIDELSOLVER) {
+				m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 1; //for mlcp solver it is better to have a small A matrix
+			} else {
+				m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128; //for direct solver, it is better to solve multiple objects together, small batches have high overhead
+			}
+
+			m_dynamicsWorld->getDispatchInfo().m_useContinuous = true; // set continuous collision
+
+		}
+		else{
+			//use btMultiBodyDynamicsWorld for Featherstone btMultiBody support
+			m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,
+				m_broadphase, (btMultiBodyConstraintSolver*) m_solver,
+				m_collisionConfiguration);
+		}
+
+		changeERPCFM(); // set appropriate ERP/CFM values according to the string and damper properties of the constraint
+
+		if (useSplitImpulse) { // If you experience strong repulsion forces in your constraints, it might help to enable the split impulse feature
+			m_dynamicsWorld->getSolverInfo().m_splitImpulse = 1; //enable split impulse feature
+			//		m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold =
+			//			-0.02;
+			//		m_dynamicsWorld->getSolverInfo().m_erp2 = BulletUtils::getERP(
+			//			fixedPhysicsStepSizeSec, 10, 1);
+			//		m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp =
+			//			BulletUtils::getERP(fixedPhysicsStepSizeSec, 10, 1);
+			//b3Printf("Using split impulse feature with ERP/TurnERP: (%f,%f)",
+			//	m_dynamicsWorld->getSolverInfo().m_erp2,
+			//	m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp);
+		}
+
+		m_dynamicsWorld->getSolverInfo().m_numIterations = gSolverIterations; // set the number of solver iterations for iteration based solvers
+
+		m_dynamicsWorld->setGravity(btVector3(0, -9.81f, 0)); // set gravity to -9.81
+
+	}
+
+	btScalar calculatePerformedSpeedup() { // calculate performed speedup
+		// we calculate the performed speed up
+		btScalar speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp));
+		//b3Printf("Avg Effective speedup: %f",speedUp);
+		performedTime = 0;
+		performanceTimestamp = mLoopTimer.getTimeMilliseconds();
+		return speedUp;
+	}
+
+
+
+	void timeWarpSimulation(float deltaTime) // Override this
+	{
+
+	}
+
+	void stepSimulation(float deltaTime){ // customly step the simulation
+		do{
+
+//			// settings
+			if(mPhysicsStepsPerSecondUpdated){
+				changePhysicsStepsPerSecond(gPhysicsStepsPerSecond);
+				mPhysicsStepsPerSecondUpdated = false;
+			}
+
+			if(mFramesPerSecondUpdated){
+				changeFPS(gFramesPerSecond);
+				mFramesPerSecondUpdated = false;
+			}
+
+			if(gChangeErpCfm){
+				changeERPCFM();
+				gChangeErpCfm = false;
+			}
+
+			if(mSolverIterationsUpdated){
+				changeSolverIterations(gSolverIterations);
+				mSolverIterationsUpdated = false;
+			}
+
+
+			// structure according to the canonical game loop
+			// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Canonical_Game_Loop
+
+			//##############
+			// breaking conditions - if the loop should stop, then check it here
+
+			//#############
+			// model update - here you perform updates of your model, be it the physics model, the game or simulation state or anything not related to graphics and input
+
+			timeWarpSimulation(deltaTime);
+			if(mLoopTimer.getTimeSeconds() - speedUpPrintTimeStamp > 1){
+				// on reset, we calculate the performed speed up
+				double speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp));
+				//b3Printf("Avg Effective speedup: %f",speedUp);
+				performedTime = 0;
+				performanceTimestamp = mLoopTimer.getTimeMilliseconds();
+				speedUpPrintTimeStamp = mLoopTimer.getTimeSeconds();
+			}
+
+			// update timers
+			mThisModelIteration = mLoopTimer.getTimeMilliseconds();
+			mFrameTime = mThisModelIteration - mPreviousModelIteration; /**!< Calculate the frame time (in Milliseconds) */
+			mPreviousModelIteration = mThisModelIteration;
+
+			//	//b3Printf("Current Frame time: % u", mFrameTime);
+
+			mApplicationRuntime = mThisModelIteration - mApplicationStart; /**!< Update main frame timer (in Milliseconds) */
+
+			mModelStart = mLoopTimer.getTimeMilliseconds(); /**!< Begin with the model update (in Milliseconds)*/
+			mLastGraphicsTick = mModelStart - mGraphicsStart; /**!< Update graphics timer (in Milliseconds) */
+
+			if (gMaximumSpeed /** If maximum speed is enabled*/) {
+				performMaxStep();
+			} else { /**!< This mode tries to progress as much time as it is expected from the game loop*/
+				performSpeedStep();
+			}
+
+			mInputStart = mLoopTimer.getTimeMilliseconds(); /**!< Start the input update */
+			mLastModelTick = mInputStart - mModelStart; /**!< Calculate the time the model update took */
+
+			//#############
+			// Input update - Game Clock part of the loop
+			/** This runs once every gApplicationTick milliseconds on average */
+			mInputDt = mThisModelIteration - mInputClock;
+			if (mInputDt >= gApplicationTick) {
+				mInputClock = mThisModelIteration;
+				//	         mInputHandler.injectInput(); /**!< Inject input into handlers */
+				//	         mInputHandler.update(mInputClock); /**!< update elements that work on the current input state */
+			}
+
+			mGraphicsStart = mLoopTimer.getTimeMilliseconds(); /**!< Start the graphics update */
+			mLastInputTick = mGraphicsStart - mInputStart; /**!< Calculate the time the input injection took */
+
+			//#############
+			// Graphics update - Here you perform the representation of your model, meaning graphics rendering according to what your game or simulation model describes
+			// In the example browser, there is a separate method called renderScene() for this
+
+			// Uncomment this for some detailed output about the application ticks
+			//	//b3Printf(
+			//		"Physics time: %u milliseconds / Graphics time: %u milliseconds / Input time: %u milliseconds / Total time passed: %u milliseconds",
+			//		mLastModelTick, mLastGraphicsTick, mLastInputTick, mApplicationRuntime);
+
+		}while(mLoopTimer.getTimeMilliseconds() - fpsTimeStamp < fpsStep); // escape the loop if it is time to render
+		// Unfortunately, the input is not included in the loop, therefore the input update frequency is equal to the fps
+
+		fpsTimeStamp = mLoopTimer.getTimeMilliseconds();
+
+	}
+
+
+	void changePhysicsStepsPerSecond(float physicsStepsPerSecond){ // change the simulation accuracy
+		if (m_dynamicsWorld && physicsStepsPerSecond) {
+			fixedPhysicsStepSizeSec = 1.0f / physicsStepsPerSecond;
+			fixedPhysicsStepSizeMilli = 1000.0f / physicsStepsPerSecond;
+
+			changeERPCFM();
+		}
+	}
+
+	void changeERPCFM(){ // Change ERP/CFM appropriately to the timestep and the ERP/CFM parameters above
+		if(m_dynamicsWorld){
+			m_dynamicsWorld->getSolverInfo().m_erp = b3ERPCFMHelper::getERP( // set the error reduction parameter
+				fixedPhysicsStepSizeSec, // step size per second
+				gERPSpringK, // k of a spring in the equation F = k * x (x:position)
+				gERPDamperC); // k of a damper in the equation F = k * v (v:velocity)
+
+			m_dynamicsWorld->getSolverInfo().m_globalCfm = b3ERPCFMHelper::getCFM( // set the constraint force mixing according to the time step
+				gCFMSingularityAvoidance, // singularity avoidance (if you experience unsolvable constraints, increase this value
+				fixedPhysicsStepSizeSec, // steps size per second
+				gCFMSpringK, // k of a spring in the equation F = k * x (x:position)
+				gCFMDamperC); // k of a damper in the equation F = k * v (v:velocity)
+
+			//b3Printf("Bullet DynamicsWorld ERP: %f",
+			//	m_dynamicsWorld->getSolverInfo().m_erp);
+
+			//b3Printf("Bullet DynamicsWorld CFM: %f",
+			//	m_dynamicsWorld->getSolverInfo().m_globalCfm);
+		}
+	}
+
+	void changeSolverIterations(int iterations){ // change the number of iterations
+		m_dynamicsWorld->getSolverInfo().m_numIterations = iterations;
+	}
+
+	void changeFPS(float framesPerSecond){ // change the frames per second
+		fpsStep = 1000.0f / gFramesPerSecond;
+	}
+
+	void performTrueSteps(btScalar timeStep){ // physics stepping without interpolated substeps
+		int subSteps = round(timeStep / fixedPhysicsStepSizeSec); /**!< Calculate the number of full normal time steps we can take */
+
+		for (int i = 0; i < subSteps; i++) { /**!< Perform the number of substeps to reach the timestep*/
+			if (timeStep && m_dynamicsWorld) {
+				// since we want to perform all proper steps, we perform no interpolated substeps
+				int subSteps = 1;
+
+				m_dynamicsWorld->stepSimulation(btScalar(timeStep),
+					btScalar(subSteps), btScalar(fixedPhysicsStepSizeSec));
+			}
+		}
+	}
+
+	void performInterpolatedSteps(btScalar timeStep){ // physics stepping with interpolated substeps
+		int subSteps = 1 + round(timeStep / fixedPhysicsStepSizeSec); /**!< Calculate the number of full normal time steps we can take, plus 1 for safety of not losing time */
+		if (timeStep && m_dynamicsWorld) {
+
+			m_dynamicsWorld->stepSimulation(btScalar(timeStep), btScalar(subSteps),
+				btScalar(fixedPhysicsStepSizeSec)); /**!< Perform the number of substeps to reach the timestep*/
+		}
+	}
+
+	void performMaxStep(){ // perform as many steps as possible
+		if(gApplicationTick >= mLastGraphicsTick + mLastInputTick){ // if the remaining time for graphics is going to be positive
+			mPhysicsTick = gApplicationTick /**!< calculate the remaining time for physics (in Milliseconds) */
+				- mLastGraphicsTick - mLastInputTick;
+		}
+		else{
+			mPhysicsTick = 0; // no time for physics left / The internal application step is too high
+		}
+
+		//	//b3Printf("Application tick: %u",gApplicationTick);
+		//	//b3Printf("Graphics tick: %u",mLastGraphicsTick);
+		//	//b3Printf("Input tick: %u",mLastInputTick);
+		//	//b3Printf("Physics tick: %u",mPhysicsTick);
+
+		if (mPhysicsTick > 0) { // with positive physics tick we perform as many update steps until the time for it is used up
+
+			mPhysicsStepStart = mLoopTimer.getTimeMilliseconds(); /**!< The physics updates start (in Milliseconds)*/
+			mPhysicsStepEnd = mPhysicsStepStart;
+
+			while (mPhysicsTick > mPhysicsStepEnd - mPhysicsStepStart) { /**!< Update the physics until we run out of time (in Milliseconds) */
+				//			//b3Printf("Physics passed: %u", mPhysicsStepEnd - mPhysicsStepStart);
+				double timeStep = fixedPhysicsStepSizeSec; /**!< update the world (in Seconds) */
+
+				if (gInterpolate) {
+					performInterpolatedSteps(timeStep);
+				} else {
+					performTrueSteps(timeStep);
+				}
+				performedTime += timeStep;
+				mPhysicsStepEnd = mLoopTimer.getTimeMilliseconds(); /**!< Update the last physics step end to stop updating in time (in Milliseconds) */
+			}
+		}
+	}
+
+
+	void performSpeedStep(){ // force-perform the number of steps needed to achieve a certain speed (safe to too high speeds, meaning the application will lose time, not the physics)
+		if (mFrameTime > gApplicationTick) { /** cap frametime to make the application lose time, not the physics (in Milliseconds) */
+			mFrameTime = gApplicationTick; // This prevents the physics time accumulator to sum up too much time
+		}				       // The simulation therefore gets slower, but still performs all requested physics steps
+
+		mModelAccumulator += mFrameTime; /**!< Accumulate the time the physics simulation has to perform in order to stay in real-time (in Milliseconds) */
+		//	//b3Printf("Model time accumulator: %u", mModelAccumulator);
+
+		int steps = floor(mModelAccumulator / fixedPhysicsStepSizeMilli); /**!< Calculate the number of time steps we can take */
+		//	//b3Printf("Next steps: %i", steps);
+
+		if (steps > 0) { /**!< Update if we can take at least one step */
+
+			double timeStep = gSimulationSpeed * steps * fixedPhysicsStepSizeSec; /**!< update the universe (in Seconds) */
+
+			if (gInterpolate) {
+				performInterpolatedSteps(timeStep); // perform interpolated steps
+			} else {
+				performTrueSteps(timeStep); // perform full steps
+			}
+			performedTime += timeStep;  // sum up the performed time for measuring the speed up
+			mModelAccumulator -= steps * fixedPhysicsStepSizeMilli; /**!< Remove the time performed by the physics simulation from the accumulator, the remaining time carries over to the next cycle  (in Milliseconds) */
+		}
+	}
+
+	void renderScene() { // render the scene
+		if(!gIsHeadless){ // while the simulation is not running headlessly, render to screen
+			CommonRigidBodyBase::renderScene();
+		}
+	}
+	void resetCamera() { // reset the camera to its original position
+		float dist = 41;
+		float pitch = 52;
+		float yaw = 35;
+		float targetPos[3] = { 0, 0.46, 0 };
+		m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1],
+			targetPos[2]);
+	}
+
+	// loop timing components ###################
+	//# loop timestamps
+	btClock mLoopTimer; /**!< The loop timer to time the loop correctly */
+	unsigned long int mApplicationStart; /**!< The time the application was started (absolute, in Milliseconds) */
+	unsigned long int mPreviousModelIteration; /**!< The previous model iteration timestamp (absolute, in Milliseconds) */
+	unsigned long int mThisModelIteration; /**!< This model iteration timestamp (absolute, in Milliseconds) */
+
+	//# loop durations
+	long int mModelAccumulator; /**!< The time to forward the model in this loop iteration (relative, in Milliseconds) */
+	unsigned long int mFrameTime; /**!< The time to render a frame (relative, in Milliseconds) */
+	unsigned long int mApplicationRuntime; /**!< The total application runtime (relative, in Milliseconds) */
+
+	long int mInputDt; /**!< The time difference of input that has to be fed in */
+	unsigned long int mInputClock;
+
+	long int mLastGraphicsTick; /*!< The time it took the graphics rendering last time (relative, in Milliseconds) */
+	unsigned long int mGraphicsStart;
+
+	long int mLastInputTick; /**!< The time it took the input to process last time (relative, in Milliseconds) */
+	unsigned long int mInputStart;
+
+	long int mLastModelTick; /**!<  The time it took the model to update last time
+	 This includes the bullet physics update */
+	unsigned long int mModelStart; /**!< The timestamp the model started updating last (absolute, in Milliseconds)*/
+
+	long int mPhysicsTick; /**!< The time remaining in the loop to update the physics (relative, in Milliseconds)*/
+	unsigned long int mPhysicsStepStart; /**!< The physics start timestamp (absolute, in Milliseconds) */
+	unsigned long int mPhysicsStepEnd; /**!< The last physics step end (absolute, in Milliseconds) */
+
+	// to measure the performance of the demo
+	double performedTime;
+	unsigned long int performanceTimestamp;
+
+	unsigned long int speedUpPrintTimeStamp;
+
+	unsigned long int fpsTimeStamp; /**!< FPS timing variables */
+	double fpsStep;
+
+	//store old values
+	bool mPhysicsStepsPerSecondUpdated;
+	bool mFramesPerSecondUpdated;
+	bool mSolverIterationsUpdated;
+};
+
+#endif //COMMON_TIME_WARP_BASE_H
+
diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 2c14758a5..b4453c8e4 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -29,7 +29,7 @@ struct btCollisionAlgorithmCreateFunc;
 class btDefaultCollisionConfiguration;
 class NNWalker;
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonTimeWarpBase.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
 #include "../Utils/b3ReferenceFrameHelper.hpp"
@@ -99,7 +99,7 @@ static btScalar gParallelEvaluations = 10.0f;
 
 void* GROUND_ID = (void*)1;
 
-class NN3DWalkersExample : public CommonRigidBodyBase
+class NN3DWalkersExample : public CommonTimeWarpBase
 {
 	btScalar m_Time;
 	btScalar m_targetAccumulator;
@@ -114,7 +114,7 @@ class NN3DWalkersExample : public CommonRigidBodyBase
 
 public:
 	NN3DWalkersExample(struct GUIHelperInterface* helper)
-	:CommonRigidBodyBase(helper),
+	:CommonTimeWarpBase(helper),
 	 m_Time(0),
 	 m_motorStrength(0.5f),
 	 m_targetFrequency(3),
@@ -150,7 +150,7 @@ public:
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 	}
 
-	virtual void renderScene();
+//	virtual void renderScene();
 
 	// Evaluation
 
@@ -557,6 +557,9 @@ void floorNNSliderValue(float notUsed) {
 
 void NN3DWalkersExample::initPhysics()
 {
+
+	setupParameterInterface(); // parameter interface to use timewarp
+
 	gContactProcessedCallback = legContactProcessedCallback;
 
 	m_guiHelper->setUpAxis(1);
@@ -788,14 +791,14 @@ void NN3DWalkersExample::exitPhysics()
 	CommonRigidBodyBase::exitPhysics();
 }
 
-void NN3DWalkersExample::renderScene()
-	{
-		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
-
-		m_guiHelper->render(m_dynamicsWorld);
-
-		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
-	}
+//void NN3DWalkersExample::renderScene()
+//{
+//		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+//
+//		m_guiHelper->render(m_dynamicsWorld);
+//
+//		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
+//}
 
 class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index fc84bb4a6..eb3f0b9e7 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -36,6 +36,8 @@ ADD_LIBRARY(BulletExampleBrowserLib
 	../Utils/b3Clock.h
 	../Utils/b3ResourcePath.cpp
 	../Utils/b3ResourcePath.h
+	../Utils/b3ERPCFMHelper.hpp
+	../Utils/b3ReferenceFrameHelper.hpp
 	${GwenGUISupport_SRCS}
 	${GwenGUISupport_HDRS}
 	
@@ -131,7 +133,7 @@ SET(ExtendedTutorialsSources
 )
 
 SET(BulletExampleBrowser_SRCS
-	
+	../CommonInterfaces/CommonTimeWarpBase.h #TODO: This is wrong here, where should it go?
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp
 	../TinyRenderer/tgaimage.cpp
diff --git a/examples/Utils/b3ERPCFMHelper.hpp b/examples/Utils/b3ERPCFMHelper.hpp
new file mode 100755
index 000000000..272616782
--- /dev/null
+++ b/examples/Utils/b3ERPCFMHelper.hpp
@@ -0,0 +1,79 @@
+#ifndef B3_ERPCFM_HELPER
+#define B3_ERPCFM_HELPER
+
+#include <LinearMath/btScalar.h>
+
+/**
+ * @brief		ERP/CFM Utils are to provide bullet specific helper functions.
+ * @details		Details
+ * @date		2015-09-20
+ * @author		Benjamin Ellenberger
+ */
+class b3ERPCFMHelper {
+public:
+
+	/**
+	 * == How To Use ERP and CFM ==
+	 * ERP and CFM can be independently set in many joints. They can be set in contact joints, in joint limits and various other places, to control the spongyness and springyness of the joint (or joint limit).
+	 * If CFM is set to zero, the constraint will be hard. If CFM is set to a positive value, it will be possible to violate the constraint by "pushing on it" (for example, for contact constraints by forcing the two contacting objects together). In other words the constraint will be soft, and the softness will increase as CFM increases. What is actually happening here is that the constraint is allowed to be violated by an amount proportional to CFM times the restoring force that is needed to enforce the constraint. Note that setting CFM to a negative value can have undesirable bad effects, such as instability. Don't do it.
+	 * By adjusting the values of ERP and CFM, you can achieve various effects. For example you can simulate springy constraints, where the two bodies oscillate as though connected by springs. Or you can simulate more spongy constraints, without the oscillation. In fact, ERP and CFM can be selected to have the same effect as any desired spring and damper constants. If you have a spring constant k_p and damping constant k_d, then the corresponding ODE constants are:
+	 *
+	 * ERP = \frac{h k_p} {h k_p + k_d}
+	 *
+	 * CFM = \frac{1} {h k_p + k_d}
+	 *
+	 * where h is the step size. These values will give the same effect as a spring-and-damper system simulated with implicit first order integration.
+	 * Increasing CFM, especially the global CFM, can reduce the numerical errors in the simulation. If the system is near-singular, then this can markedly increase stability. In fact, if the system is misbehaving, one of the first things to try is to increase the global CFM.
+	 * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM
+	 * @return
+	 */
+	/**
+	 * Joint error and the Error Reduction Parameter (ERP)
+	 *
+	 * When a joint attaches two bodies, those bodies are required to have certain positions and orientations relative to each other. However, it is possible for the bodies to be in positions where the joint constraints are not met. This "joint error" can happen in two ways:
+	 *
+	 * If the user sets the position/orientation of one body without correctly setting the position/orientation of the other body.
+	 * During the simulation, errors can creep in that result in the bodies drifting away from their required positions.
+	 * Figure 3  shows an example of error in a ball and socket joint (where the ball and socket do not line up).
+	 *
+	 * There is a mechanism to reduce joint error: during each simulation step each joint applies a special force to bring its bodies back into correct alignment. This force is controlled by the error reduction parameter (ERP), which has a value between 0 and 1.
+	 *
+	 * The ERP specifies what proportion of the joint error will be fixed during the next simulation step. If ERP = 0 then no correcting force is applied and the bodies will eventually drift apart as the simulation proceeds. If ERP=1 then the simulation will attempt to fix all joint error during the next time step. However, setting ERP=1 is not recommended, as the joint error will not be completely fixed due to various internal approximations. A value of ERP=0.1 to 0.8 is recommended (0.2 is the default).
+	 *
+	 * A global ERP value can be set that affects most joints in the simulation. However some joints have local ERP values that control various aspects of the joint.
+	 * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM
+	 * @return
+	 */
+	static btScalar getERP(btScalar timeStep, btScalar kSpring,
+		btScalar kDamper) {
+		return timeStep * kSpring / (timeStep * kSpring + kDamper);
+	}
+
+	/**
+	 * Most constraints are by nature "hard". This means that the constraints represent conditions that are never violated. For example, the ball must always be in the socket, and the two parts of the hinge must always be lined up. In practice constraints can be violated by unintentional introduction of errors into the system, but the error reduction parameter can be set to correct these errors.
+	 * Not all constraints are hard. Some "soft" constraints are designed to be violated. For example, the contact constraint that prevents colliding objects from penetrating is hard by default, so it acts as though the colliding surfaces are made of steel. But it can be made into a soft constraint to simulate softer materials, thereby allowing some natural penetration of the two objects when they are forced together.
+	 * There are two parameters that control the distinction between hard and soft constraints. The first is the error reduction parameter (ERP) that has already been introduced. The second is the constraint force mixing (CFM) value, that is described below.
+	 *
+	 * == Constraint Force Mixing (CFM) ==
+	 * What follows is a somewhat technical description of the meaning of CFM. If you just want to know how it is used in practice then skip to the next section.
+	 * Traditionally the constraint equation for every joint has the form
+	 * \mathbf{J} v = \mathbf{c}
+	 * where v is a velocity vector for the bodies involved, \mathbf{J} is a "Jacobian" matrix with one row for every degree of freedom the joint removes from the system, and \mathbf{c} is a right hand side vector. At the next time step, a vector \lambda is calculated (of the same size as \mathbf{c}) such that the forces applied to the bodies to preserve the joint constraint are:
+	 * F_c = \mathbf{J}^T \lambda
+	 * ODE adds a new twist. ODE's constraint equation has the form
+	 *
+	 * \mathbf{J} v = \mathbf{c} + \textbf{CFM} \, \lambda
+	 * where CFM is a square diagonal matrix. CFM mixes the resulting constraint force in with the constraint that produces it. A nonzero (positive) value of CFM allows the original constraint equation to be violated by an amount proportional to CFM times the restoring force \lambda that is needed to enforce the constraint. Solving for \lambda gives
+	 *
+	 * (\mathbf{J} \mathbf{M}^{-1} \mathbf{J}^T + \frac{1}{h}  \textbf{CFM}) \lambda = \frac{1}{h} \mathbf{c}
+	 * Thus CFM simply adds to the diagonal of the original system matrix. Using a positive value of CFM has the additional benefit of taking the system away from any singularity and thus improving the factorizer accuracy.
+	 * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM
+	 * @return
+	 */
+	static btScalar getCFM(btScalar avoidSingularity, btScalar timeStep, btScalar kSpring,
+		btScalar kDamper) {
+		return btScalar(avoidSingularity) / (timeStep * kSpring + kDamper);
+	}
+};
+
+#endif /* B3_ERPCFM_HELPER */

From 7cf4a2352ccf03be9982ea75b51e97be0eafc8be Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Sun, 18 Sep 2016 00:42:34 +0200
Subject: [PATCH 17/22] Simplify the parameter interface for NNWalkers. Keys
 1-9 cause speedup.

---
 .../CommonInterfaces/CommonTimeWarpBase.h     | 246 +++++++++++-------
 examples/Evolution/NN3DWalkers.cpp            |  54 ++--
 2 files changed, 185 insertions(+), 115 deletions(-)

diff --git a/examples/CommonInterfaces/CommonTimeWarpBase.h b/examples/CommonInterfaces/CommonTimeWarpBase.h
index 371b79997..09ecec039 100755
--- a/examples/CommonInterfaces/CommonTimeWarpBase.h
+++ b/examples/CommonInterfaces/CommonTimeWarpBase.h
@@ -16,8 +16,6 @@
 #ifndef COMMON_TIME_WARP_BASE_H
 #define COMMON_TIME_WARP_BASE_H
 
-#include <string>
-
 #include "btBulletDynamicsCommon.h"
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedObjectArray.h"
@@ -25,14 +23,16 @@
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
-#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
-#include <BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h>
-#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
-#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
-#include <BulletDynamics/MLCPSolvers/btDantzigSolver.h>
-#include <BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h>
-#include <BulletDynamics/MLCPSolvers/btLemkeSolver.h>
-#include <BulletDynamics/MLCPSolvers/btMLCPSolver.h>
+
+//Solvers
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
 
 #include "../Utils/b3ERPCFMHelper.hpp" // ERP/CFM setting utils
 
@@ -55,19 +55,20 @@ static double /**/NUM_SPEEDS = 11;
 };
 
 // add speeds from the namespace here
-static double speeds[] = { SimulationSpeeds::PAUSE,
-		SimulationSpeeds::QUARTER_SPEED, SimulationSpeeds::HALF_SPEED,
-		SimulationSpeeds::NORMAL_SPEED, SimulationSpeeds::DOUBLE_SPEED,
-		SimulationSpeeds::QUADRUPLE_SPEED, SimulationSpeeds::DECUPLE_SPEED,
-		SimulationSpeeds::CENTUPLE_SPEED,SimulationSpeeds::QUINCENTUPLE_SPEED,
-		SimulationSpeeds::MILLITUPLE_SPEED};
+static double speeds[] = {
+	SimulationSpeeds::PAUSE,
+	SimulationSpeeds::QUARTER_SPEED, 	SimulationSpeeds::HALF_SPEED,
+	SimulationSpeeds::NORMAL_SPEED, 	SimulationSpeeds::DOUBLE_SPEED,
+	SimulationSpeeds::QUADRUPLE_SPEED, 	SimulationSpeeds::DECUPLE_SPEED,
+	SimulationSpeeds::CENTUPLE_SPEED,	SimulationSpeeds::QUINCENTUPLE_SPEED,
+	SimulationSpeeds::MILLITUPLE_SPEED};
 
 static btScalar gSolverIterations = 10; // default number of solver iterations for the iterative solvers
 
-static bool gChangeErpCfm = false;
-
 static bool gIsHeadless = false; // demo runs with graphics by default
 
+static bool gChangeErpCfm = false; // flag to make recalculation of ERP/CFM
+
 static int gMinSpeed = SimulationSpeeds::PAUSE; // the minimum simulation speed
 
 static int gMaxSpeed = SimulationSpeeds::MAX_SPEED; // the maximum simulation speed
@@ -93,7 +94,7 @@ enum SolverEnumType {
 };
 
 
-//TODO: In case the solver should be changeable by drop down menu
+// solvers can be changed by drop down menu
 namespace SolverType {
 static char SEQUENTIALIMPULSESOLVER[] = "Sequential Impulse Solver";
 static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver";
@@ -157,11 +158,11 @@ inline void changeSolver(int comboboxId, const char* item, void* userPointer) {
 	for(int i = 0; i < NUM_SOLVERS;i++){
 		if(strcmp(solverTypes[i], item) == 0){ // if the strings are equal
 			SOLVER_TYPE = ((SolverEnumType)i);
-			//b3Printf("=%s=\n Reset the simulation by double clicking it in the menu list.",item);
+			b3Printf("=%s=\n Reset the simulation by double clicking it in the menu list.",item);
 			return;
 		}
 	}
-	//b3Printf("No Change");
+	b3Printf("No Change");
 }
 
 
@@ -187,17 +188,16 @@ inline void clampToCustomSpeedNotches(float speed) { // function to clamp to cus
 
 inline void switchInterpolated(int buttonId, bool buttonState, void* userPointer){ // toggle if interpolation steps are taken
 	gInterpolate=!gInterpolate;
-	//b3Printf("Interpolate substeps %s", gInterpolate?"on":"off");
+//	b3Printf("Interpolate substeps %s", gInterpolate?"on":"off");
 }
 
 inline void switchHeadless(int buttonId, bool buttonState, void* userPointer){ // toggle if the demo should run headless
 	gIsHeadless=!gIsHeadless;
-	//b3Printf("Run headless %s", gIsHeadless?"on":"off");
+//	b3Printf("Run headless %s", gIsHeadless?"on":"off");
 }
 
 inline void switchMaximumSpeed(int buttonId, bool buttonState, void* userPointer){ // toggle it the demo should run as fast as possible
-	gMaximumSpeed=!gMaximumSpeed;
-	//b3Printf("Run maximum speed %s", gMaximumSpeed?"on":"off");
+//	b3Printf("Run maximum speed %s", gMaximumSpeed?"on":"off");
 }
 
 inline void setApplicationTick(float frequency){ // set internal application tick
@@ -257,7 +257,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 
 	void initPhysics(){ // initialize the demo
 
-		setupParameterInterface(); // setup adjustable sliders and buttons for parameters
+		setupBasicParamInterface(); // setup adjustable sliders and buttons for parameters
 
 		m_guiHelper->setUpAxis(1); // Set Y axis as Up axis
 
@@ -266,25 +266,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 		m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 	}
 
-	void setupParameterInterface(){ // setup the adjustable sliders and button for parameters
-
-		solverTypes[0] = SolverType::SEQUENTIALIMPULSESOLVER;
-		solverTypes[1] = SolverType::GAUSSSEIDELSOLVER;
-		solverTypes[2] = SolverType::NNCGSOLVER;
-		solverTypes[3] = SolverType::DANZIGSOLVER;
-		solverTypes[4] = SolverType::LEMKESOLVER;
-		solverTypes[5] = SolverType::FSSOLVER;
-
-		{
-			ComboBoxParams comboParams;
-			comboParams.m_comboboxId = 0;
-			comboParams.m_numItems = NUM_SOLVERS;
-			comboParams.m_startItem = SOLVER_TYPE;
-			comboParams.m_callback = changeSolver;
-
-			comboParams.m_items=solverTypes;
-			m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
-		}
+	void setupBasicParamInterface(){ // setup the adjustable sliders and button for parameters
 
 		{ // create a slider to adjust the simulation speed
 			// Force increase the simulation speed to run the simulation with the same accuracy but a higher speed
@@ -308,6 +290,50 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 					button);
 		}
 
+
+
+		{ // create a button to switch to maximum speed simulation (fully deterministic)
+			// Interesting to test the maximal achievable speed on this hardware
+			ButtonParams button("Run maximum speed",0,true);
+			button.m_callback = switchMaximumSpeed;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerButtonParameter(
+					button);
+		}
+
+
+
+		{ // create a button to switch bullet to perform interpolated substeps to speed up simulation
+			// generally, interpolated steps are a good speed-up and should only be avoided if higher accuracy is needed (research purposes etc.)
+			ButtonParams button("Perform interpolated substeps",0,true);
+			button.m_callback = switchInterpolated;
+			if (m_guiHelper->getParameterInterface())
+				m_guiHelper->getParameterInterface()->registerButtonParameter(
+					button);
+		}
+
+	}
+
+	void setupAdvancedParamInterface(){
+
+		solverTypes[0] = SolverType::SEQUENTIALIMPULSESOLVER;
+		solverTypes[1] = SolverType::GAUSSSEIDELSOLVER;
+		solverTypes[2] = SolverType::NNCGSOLVER;
+		solverTypes[3] = SolverType::DANZIGSOLVER;
+		solverTypes[4] = SolverType::LEMKESOLVER;
+		solverTypes[5] = SolverType::FSSOLVER;
+
+		{
+			ComboBoxParams comboParams;
+			comboParams.m_comboboxId = 0;
+			comboParams.m_numItems = NUM_SOLVERS;
+			comboParams.m_startItem = SOLVER_TYPE;
+			comboParams.m_callback = changeSolver;
+
+			comboParams.m_items=solverTypes;
+			m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+		}
+
 		{ // create a slider to adjust the number of internal application ticks
 			// The set application tick should contain enough time to perform a full cycle of model update (physics and input)
 			// and view update (graphics) with average application load. The graphics and input update determine the remaining time
@@ -323,15 +349,6 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 					slider);
 		}
 
-		{ // create a button to switch to maximum speed simulation (fully deterministic)
-			// Interesting to test the maximal achievable speed on this hardware
-			ButtonParams button("Run maximum speed",0,true);
-			button.m_callback = switchMaximumSpeed;
-			if (m_guiHelper->getParameterInterface())
-				m_guiHelper->getParameterInterface()->registerButtonParameter(
-					button);
-		}
-
 		{ // create a slider to adjust the number of physics steps per second
 			// The default number of steps is at 60, which is appropriate for most general simulations
 			// For simulations with higher complexity or if you experience undesired behavior, try increasing the number of steps per second
@@ -357,15 +374,6 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 					slider);
 		}
 
-		{ // create a button to switch bullet to perform interpolated substeps to speed up simulation
-			// generally, interpolated steps are a good speed-up and should only be avoided if higher accuracy is needed (research purposes etc.)
-			ButtonParams button("Perform interpolated substeps",0,true);
-			button.m_callback = switchInterpolated;
-			if (m_guiHelper->getParameterInterface())
-				m_guiHelper->getParameterInterface()->registerButtonParameter(
-					button);
-		}
-
 		{ // create a slider to adjust the number of solver iterations to converge to a solution
 			// more complex simulations might need a higher number of iterations to converge, it also
 			// depends on the type of solver.
@@ -437,6 +445,8 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
 					slider);
 		}
+
+
 	}
 
 	void createEmptyDynamicsWorld(){ // create an empty dynamics worlds according to the chosen settings via statics (top section of code)
@@ -454,35 +464,35 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 		// different solvers require different settings
 		switch (SOLVER_TYPE) {
 		case SEQUENTIALIMPULSESOLVER: {
-			//b3Printf("=%s=",SolverType::SEQUENTIALIMPULSESOLVER);
+//			b3Printf("=%s=",SolverType::SEQUENTIALIMPULSESOLVER);
 			m_solver = new btSequentialImpulseConstraintSolver();
 			break;
 		}
 		case NNCGSOLVER: {
-			//b3Printf("=%s=",SolverType::NNCGSOLVER);
+//			b3Printf("=%s=",SolverType::NNCGSOLVER);
 			m_solver = new btNNCGConstraintSolver();
 			break;
 		}
 		case DANZIGSOLVER: {
-			//b3Printf("=%s=",SolverType::DANZIGSOLVER);
+//			b3Printf("=%s=",SolverType::DANZIGSOLVER);
 			btDantzigSolver* mlcp = new btDantzigSolver();
 			m_solver = new btMLCPSolver(mlcp);
 			break;
 		}
 		case GAUSSSEIDELSOLVER: {
-			//b3Printf("=%s=",SolverType::GAUSSSEIDELSOLVER);
+//			b3Printf("=%s=",SolverType::GAUSSSEIDELSOLVER);
 			btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
 			m_solver = new btMLCPSolver(mlcp);
 			break;
 		}
 		case LEMKESOLVER: {
-			//b3Printf("=%s=",SolverType::LEMKESOLVER);
+//			b3Printf("=%s=",SolverType::LEMKESOLVER);
 			btLemkeSolver* mlcp = new btLemkeSolver();
 			m_solver = new btMLCPSolver(mlcp);
 			break;
 		}
 		case FSSOLVER: {
-			//b3Printf("=%s=",SolverType::FSSOLVER);
+//			b3Printf("=%s=",SolverType::FSSOLVER);
 			//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
 			m_solver = new btMultiBodyConstraintSolver;
 
@@ -524,9 +534,9 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			//			fixedPhysicsStepSizeSec, 10, 1);
 			//		m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp =
 			//			BulletUtils::getERP(fixedPhysicsStepSizeSec, 10, 1);
-			//b3Printf("Using split impulse feature with ERP/TurnERP: (%f,%f)",
-			//	m_dynamicsWorld->getSolverInfo().m_erp2,
-			//	m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp);
+//			b3Printf("Using split impulse feature with ERP/TurnERP: (%f,%f)",
+//				m_dynamicsWorld->getSolverInfo().m_erp2,
+//				m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp);
 		}
 
 		m_dynamicsWorld->getSolverInfo().m_numIterations = gSolverIterations; // set the number of solver iterations for iteration based solvers
@@ -538,7 +548,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 	btScalar calculatePerformedSpeedup() { // calculate performed speedup
 		// we calculate the performed speed up
 		btScalar speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp));
-		//b3Printf("Avg Effective speedup: %f",speedUp);
+//		b3Printf("Avg Effective speedup: %f",speedUp);
 		performedTime = 0;
 		performanceTimestamp = mLoopTimer.getTimeMilliseconds();
 		return speedUp;
@@ -589,7 +599,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			if(mLoopTimer.getTimeSeconds() - speedUpPrintTimeStamp > 1){
 				// on reset, we calculate the performed speed up
 				double speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp));
-				//b3Printf("Avg Effective speedup: %f",speedUp);
+//				b3Printf("Avg Effective speedup: %f",speedUp);
 				performedTime = 0;
 				performanceTimestamp = mLoopTimer.getTimeMilliseconds();
 				speedUpPrintTimeStamp = mLoopTimer.getTimeSeconds();
@@ -600,7 +610,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			mFrameTime = mThisModelIteration - mPreviousModelIteration; /**!< Calculate the frame time (in Milliseconds) */
 			mPreviousModelIteration = mThisModelIteration;
 
-			//	//b3Printf("Current Frame time: % u", mFrameTime);
+			//	b3Printf("Current Frame time: % u", mFrameTime);
 
 			mApplicationRuntime = mThisModelIteration - mApplicationStart; /**!< Update main frame timer (in Milliseconds) */
 
@@ -634,7 +644,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			// In the example browser, there is a separate method called renderScene() for this
 
 			// Uncomment this for some detailed output about the application ticks
-			//	//b3Printf(
+			//	b3Printf(
 			//		"Physics time: %u milliseconds / Graphics time: %u milliseconds / Input time: %u milliseconds / Total time passed: %u milliseconds",
 			//		mLastModelTick, mLastGraphicsTick, mLastInputTick, mApplicationRuntime);
 
@@ -645,6 +655,64 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 
 	}
 
+	virtual bool	keyboardCallback(int key, int state)
+	{
+		switch(key)
+		{
+		case '1':{
+			gSimulationSpeed = SimulationSpeeds::QUARTER_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '2':{
+			gSimulationSpeed = SimulationSpeeds::HALF_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '3':{
+			gSimulationSpeed = SimulationSpeeds::NORMAL_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '4':{
+			gSimulationSpeed = SimulationSpeeds::DOUBLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '5':{
+			gSimulationSpeed = SimulationSpeeds::QUADRUPLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '6':{
+			gSimulationSpeed = SimulationSpeeds::DECUPLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '7':{
+			gSimulationSpeed = SimulationSpeeds::CENTUPLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '8':{
+			gSimulationSpeed = SimulationSpeeds::QUINCENTUPLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '9':{
+			gSimulationSpeed = SimulationSpeeds::MILLITUPLE_SPEED;
+			gMaximumSpeed = false;
+			return true;
+		}
+		case '0':{
+			gSimulationSpeed = SimulationSpeeds::MAX_SPEED;
+			gMaximumSpeed = true;
+			return true;
+		}
+		}
+		return CommonRigidBodyBase::keyboardCallback(key,state);
+	}
+
 
 	void changePhysicsStepsPerSecond(float physicsStepsPerSecond){ // change the simulation accuracy
 		if (m_dynamicsWorld && physicsStepsPerSecond) {
@@ -668,11 +736,11 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 				gCFMSpringK, // k of a spring in the equation F = k * x (x:position)
 				gCFMDamperC); // k of a damper in the equation F = k * v (v:velocity)
 
-			//b3Printf("Bullet DynamicsWorld ERP: %f",
-			//	m_dynamicsWorld->getSolverInfo().m_erp);
+//			b3Printf("Bullet DynamicsWorld ERP: %f",
+//				m_dynamicsWorld->getSolverInfo().m_erp);
 
-			//b3Printf("Bullet DynamicsWorld CFM: %f",
-			//	m_dynamicsWorld->getSolverInfo().m_globalCfm);
+//			b3Printf("Bullet DynamicsWorld CFM: %f",
+//				m_dynamicsWorld->getSolverInfo().m_globalCfm);
 		}
 	}
 
@@ -716,10 +784,10 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			mPhysicsTick = 0; // no time for physics left / The internal application step is too high
 		}
 
-		//	//b3Printf("Application tick: %u",gApplicationTick);
-		//	//b3Printf("Graphics tick: %u",mLastGraphicsTick);
-		//	//b3Printf("Input tick: %u",mLastInputTick);
-		//	//b3Printf("Physics tick: %u",mPhysicsTick);
+		//	b3Printf("Application tick: %u",gApplicationTick);
+		//	b3Printf("Graphics tick: %u",mLastGraphicsTick);
+		//	b3Printf("Input tick: %u",mLastInputTick);
+		//	b3Printf("Physics tick: %u",mPhysicsTick);
 
 		if (mPhysicsTick > 0) { // with positive physics tick we perform as many update steps until the time for it is used up
 
@@ -727,7 +795,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 			mPhysicsStepEnd = mPhysicsStepStart;
 
 			while (mPhysicsTick > mPhysicsStepEnd - mPhysicsStepStart) { /**!< Update the physics until we run out of time (in Milliseconds) */
-				//			//b3Printf("Physics passed: %u", mPhysicsStepEnd - mPhysicsStepStart);
+				//			b3Printf("Physics passed: %u", mPhysicsStepEnd - mPhysicsStepStart);
 				double timeStep = fixedPhysicsStepSizeSec; /**!< update the world (in Seconds) */
 
 				if (gInterpolate) {
@@ -748,10 +816,10 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 		}				       // The simulation therefore gets slower, but still performs all requested physics steps
 
 		mModelAccumulator += mFrameTime; /**!< Accumulate the time the physics simulation has to perform in order to stay in real-time (in Milliseconds) */
-		//	//b3Printf("Model time accumulator: %u", mModelAccumulator);
+		//	b3Printf("Model time accumulator: %u", mModelAccumulator);
 
 		int steps = floor(mModelAccumulator / fixedPhysicsStepSizeMilli); /**!< Calculate the number of time steps we can take */
-		//	//b3Printf("Next steps: %i", steps);
+		//	b3Printf("Next steps: %i", steps);
 
 		if (steps > 0) { /**!< Update if we can take at least one step */
 
@@ -770,6 +838,10 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 	void renderScene() { // render the scene
 		if(!gIsHeadless){ // while the simulation is not running headlessly, render to screen
 			CommonRigidBodyBase::renderScene();
+
+			if(m_dynamicsWorld->getDebugDrawer()){
+				debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
+			}
 		}
 	}
 	void resetCamera() { // reset the camera to its original position
diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index b4453c8e4..e79437f7f 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -102,6 +102,7 @@ void* GROUND_ID = (void*)1;
 class NN3DWalkersExample : public CommonTimeWarpBase
 {
 	btScalar m_Time;
+	btScalar m_SpeedupTimestamp;
 	btScalar m_targetAccumulator;
 	btScalar m_targetFrequency;
 	btScalar m_motorStrength;
@@ -116,6 +117,7 @@ public:
 	NN3DWalkersExample(struct GUIHelperInterface* helper)
 	:CommonTimeWarpBase(helper),
 	 m_Time(0),
+	 m_SpeedupTimestamp(0),
 	 m_motorStrength(0.5f),
 	 m_targetFrequency(3),
 	 m_targetAccumulator(0),
@@ -150,8 +152,6 @@ public:
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 	}
 
-//	virtual void renderScene();
-
 	// Evaluation
 
 	void update(const btScalar timeSinceLastTick);
@@ -520,7 +520,9 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
 	    // Make a circle with a 0.9 radius at (0,0,0)
 	    // with RGB color (1,0,0).
 		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL){
-			nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
+			if(!gIsHeadless){
+				nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
+			}
 		}
 
 		if(ID1 != GROUND_ID && ID1){
@@ -558,7 +560,7 @@ void floorNNSliderValue(float notUsed) {
 void NN3DWalkersExample::initPhysics()
 {
 
-	setupParameterInterface(); // parameter interface to use timewarp
+	setupBasicParamInterface(); // parameter interface to use timewarp
 
 	gContactProcessedCallback = legContactProcessedCallback;
 
@@ -760,19 +762,17 @@ bool NN3DWalkersExample::keyboardCallback(int key, int state)
 	case '[':
 		m_motorStrength /= 1.1f;
 		return true;
-		break;
 	case ']':
 		m_motorStrength *= 1.1f;
 		return true;
-		break;
 	case 'l':
 		printWalkerConfigs();
-		break;
+		return true;
 	default:
 		break;
 	}
 
-	return false;
+	return CommonTimeWarpBase::keyboardCallback(key,state);
 }
 
 void NN3DWalkersExample::exitPhysics()
@@ -791,15 +791,6 @@ void NN3DWalkersExample::exitPhysics()
 	CommonRigidBodyBase::exitPhysics();
 }
 
-//void NN3DWalkersExample::renderScene()
-//{
-//		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
-//
-//		m_guiHelper->render(m_dynamicsWorld);
-//
-//		debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
-//}
-
 class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
 {
 	nn3DWalkers = new NN3DWalkersExample(options.m_guiHelper);
@@ -919,6 +910,11 @@ void NN3DWalkersExample::update(const btScalar timeSinceLastTick) {
 	scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */
 
 	drawMarkings(); /**!< Draw markings on the ground */
+
+//	if(m_Time > m_SpeedupTimestamp + 1.0f){ // print effective speedup
+//		b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup());
+//		m_SpeedupTimestamp = m_Time;
+//	}
 }
 
 void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
@@ -1020,19 +1016,21 @@ void NN3DWalkersExample::scheduleEvaluations() {
 }
 
 void NN3DWalkersExample::drawMarkings() {
-	for(int i = 0; i < NUM_WALKERS;i++) // draw current distance plates of moving walkers
-	{
-		if(m_walkersInPopulation[i]->isInEvaluation()){
-			btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
-			char performance[10];
-			sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
-			m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
+	if(!gIsHeadless){
+		for(int i = 0; i < NUM_WALKERS;i++) // draw current distance plates of moving walkers
+		{
+			if(m_walkersInPopulation[i]->isInEvaluation()){
+				btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
+				char performance[10];
+				sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
+				m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
+			}
 		}
-	}
 
-	for(int i = 2; i < 50; i+=2){ // draw distance circles
-		if(m_dynamicsWorld->getDebugDrawer()){
-			m_dynamicsWorld->getDebugDrawer()->drawArc(btVector3(0,0,0),btVector3(0,1,0),btVector3(1,0,0),btScalar(i), btScalar(i),btScalar(0),btScalar(SIMD_2_PI),btVector3(10*i,0,0),false);
+		for(int i = 2; i < 50; i+=2){ // draw distance circles
+			if(m_dynamicsWorld->getDebugDrawer()){
+				m_dynamicsWorld->getDebugDrawer()->drawArc(btVector3(0,0,0),btVector3(0,1,0),btVector3(1,0,0),btScalar(i), btScalar(i),btScalar(0),btScalar(SIMD_2_PI),btVector3(10*i,0,0),false);
+			}
 		}
 	}
 }

From 51e51ca848ac8a2b98e4296c0bbb71436577efce Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Sun, 18 Sep 2016 00:52:08 +0200
Subject: [PATCH 18/22] Print out effective speedup. Reduce application tick to
 60/s.

---
 examples/CommonInterfaces/CommonTimeWarpBase.h | 2 +-
 examples/Evolution/NN3DWalkers.cpp             | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/examples/CommonInterfaces/CommonTimeWarpBase.h b/examples/CommonInterfaces/CommonTimeWarpBase.h
index 09ecec039..3c01cba9e 100755
--- a/examples/CommonInterfaces/CommonTimeWarpBase.h
+++ b/examples/CommonInterfaces/CommonTimeWarpBase.h
@@ -124,7 +124,7 @@ static btScalar gPhysicsStepsPerSecond = 60.0f; // Default number of steps
 static double fixedPhysicsStepSizeSec = 1.0f / gPhysicsStepsPerSecond; // steps size in seconds
 static double fixedPhysicsStepSizeMilli = 1000.0f / gPhysicsStepsPerSecond; // step size in milliseconds
 
-static btScalar gApplicationFrequency = 120.0f; // number of internal application ticks per second
+static btScalar gApplicationFrequency = 60.0f; // number of internal application ticks per second
 static int gApplicationTick = 1000.0f / gApplicationFrequency; //ms
 
 static btScalar gFramesPerSecond = 30.0f; // number of frames per second
diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index e79437f7f..06cb14c28 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -911,10 +911,10 @@ void NN3DWalkersExample::update(const btScalar timeSinceLastTick) {
 
 	drawMarkings(); /**!< Draw markings on the ground */
 
-//	if(m_Time > m_SpeedupTimestamp + 1.0f){ // print effective speedup
-//		b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup());
-//		m_SpeedupTimestamp = m_Time;
-//	}
+	if(m_Time > m_SpeedupTimestamp + 2.0f){ // print effective speedup
+		b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup());
+		m_SpeedupTimestamp = m_Time;
+	}
 }
 
 void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {

From 062236af700cff3b4e452f081e2c0ade8867f154 Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Sun, 18 Sep 2016 01:03:41 +0200
Subject: [PATCH 19/22] Fix running headless to be nearly completely headless.

---
 examples/CommonInterfaces/CommonTimeWarpBase.h | 2 ++
 examples/Evolution/NN3DWalkers.cpp             | 4 ++--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/examples/CommonInterfaces/CommonTimeWarpBase.h b/examples/CommonInterfaces/CommonTimeWarpBase.h
index 3c01cba9e..dde06b417 100755
--- a/examples/CommonInterfaces/CommonTimeWarpBase.h
+++ b/examples/CommonInterfaces/CommonTimeWarpBase.h
@@ -843,6 +843,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 				debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode());
 			}
 		}
+		mIsHeadless = gIsHeadless;
 	}
 	void resetCamera() { // reset the camera to its original position
 		float dist = 41;
@@ -895,6 +896,7 @@ struct CommonTimeWarpBase: public CommonRigidBodyBase {
 	bool mPhysicsStepsPerSecondUpdated;
 	bool mFramesPerSecondUpdated;
 	bool mSolverIterationsUpdated;
+	bool mIsHeadless;
 };
 
 #endif //COMMON_TIME_WARP_BASE_H
diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 06cb14c28..52217b5c6 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -520,7 +520,7 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
 	    // Make a circle with a 0.9 radius at (0,0,0)
 	    // with RGB color (1,0,0).
 		if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL){
-			if(!gIsHeadless){
+			if(!nn3DWalkers->mIsHeadless){
 				nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
 			}
 		}
@@ -1016,7 +1016,7 @@ void NN3DWalkersExample::scheduleEvaluations() {
 }
 
 void NN3DWalkersExample::drawMarkings() {
-	if(!gIsHeadless){
+	if(!mIsHeadless){
 		for(int i = 0; i < NUM_WALKERS;i++) // draw current distance plates of moving walkers
 		{
 			if(m_walkersInPopulation[i]->isInEvaluation()){

From 134c788f93b61b52a78ea9a1f65ebfa20528b6a2 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <benjamin.ellenberger@intrasoft.ch>
Date: Wed, 19 Oct 2016 18:35:01 +0200
Subject: [PATCH 20/22] Change all btVector/btTransform to const
 btVector/btTransform& for SIMD alignment.

---
 examples/Evolution/NN3DWalkers.cpp        |  2 +-
 examples/Utils/b3ReferenceFrameHelper.hpp | 12 ++++++------
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index 52217b5c6..cba5abb0c 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -460,7 +460,7 @@ public:
 		return getDistanceFitness(); // for now it is only distance
 	}
 
-	void resetAt(btVector3 position) {
+	void resetAt(const btVector3& position) {
 		btTransform resetPosition(btQuaternion::getIdentity(), position);
 		for (int i = 0; i < BODYPART_COUNT; ++i)
 		{
diff --git a/examples/Utils/b3ReferenceFrameHelper.hpp b/examples/Utils/b3ReferenceFrameHelper.hpp
index 06f63f087..05e839d53 100755
--- a/examples/Utils/b3ReferenceFrameHelper.hpp
+++ b/examples/Utils/b3ReferenceFrameHelper.hpp
@@ -21,33 +21,33 @@ subject to the following restrictions:
 
 class b3ReferenceFrameHelper {
 public:
-	static btVector3 getPointWorldToLocal( btTransform localObjectCenterOfMassTransform, btVector3 point) {
+	static btVector3 getPointWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btVector3& point) {
 		  return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame
 	}
 
-	static btVector3 getPointLocalToWorld( btTransform localObjectCenterOfMassTransform, btVector3 point) {
+	static btVector3 getPointLocalToWorld(const btTransform& localObjectCenterOfMassTransform,const btVector3& point) {
 		  return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame
 	}
 
-	static btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
+	static btVector3 getAxisWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btVector3& axis) {
 	  btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame
 	  btVector3 zero(0,0,0);
 	  local1.setOrigin(zero);
 	  return local1 * axis;
 	}
 
-	static btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 axis) {
+	static btVector3 getAxisLocalToWorld(const btTransform& localObjectCenterOfMassTransform, const btVector3& axis) {
 	  btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame
 	  btVector3 zero(0,0,0);
 	  local1.setOrigin(zero);
 	  return local1 * axis;
 	}
 
-	static btTransform getTransformWorldToLocal(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+	static btTransform getTransformWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btTransform& transform) {
 	  return localObjectCenterOfMassTransform.inverse() * transform; // transforms the axis from the local frame into the world frame
 	}
 
-	static btTransform getTransformLocalToWorld(btTransform localObjectCenterOfMassTransform, btTransform transform) {
+	static btTransform getTransformLocalToWorld(const btTransform& localObjectCenterOfMassTransform, const btTransform& transform) {
 	  return localObjectCenterOfMassTransform * transform; // transforms the axis from the local frame into the world frame
 	}
 

From 453b0f9e29e532fe2e5cdf7eba0e0f6bdfa9d102 Mon Sep 17 00:00:00 2001
From: Benjamin Ellenberger <benjamin.ellenberger@intrasoft.ch>
Date: Wed, 19 Oct 2016 21:20:57 +0200
Subject: [PATCH 21/22] Fix uninitialized transform. Increase performance
 string. Remove unused method signature.

---
 examples/Evolution/NN3DWalkers.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index cba5abb0c..d7322b59f 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -138,9 +138,7 @@ public:
 	void spawnWalker(int index, const btVector3& startOffset, bool bFixed);
 	
 	virtual bool	keyboardCallback(int key, int state);
-	
-	void setMotorTargets(btScalar deltaTime);
-	
+
 	bool detectCollisions();
 
 	void resetCamera()
@@ -269,7 +267,7 @@ public:
 
 		m_bodies[0] = localCreateRigidBody(btScalar(bFixed?0.:1.), bodyOffset*transform, m_shapes[0]);
 		m_ownerWorld->addRigidBody(m_bodies[0]);
-		m_bodyRelativeTransforms[0] = btTransform();
+		m_bodyRelativeTransforms[0] = btTransform::getIdentity();
 		m_bodies[0]->setUserPointer(this);
 		m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[0], 0));
 
@@ -1021,7 +1019,7 @@ void NN3DWalkersExample::drawMarkings() {
 		{
 			if(m_walkersInPopulation[i]->isInEvaluation()){
 				btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
-				char performance[10];
+				char performance[20];
 				sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
 				m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),3);
 			}

From 09d2e9afa7dd80cb72870315ae7ad728e7ca6674 Mon Sep 17 00:00:00 2001
From: Benelot <be.ellenberger@gmail.com>
Date: Wed, 19 Oct 2016 21:56:09 +0200
Subject: [PATCH 22/22] Replace std::map with btHashMap.

---
 examples/Evolution/NN3DWalkers.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp
index d7322b59f..fb6f01cf4 100755
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -14,12 +14,12 @@ subject to the following restrictions:
 */
 
 #include "NN3DWalkers.h"
-#include <map>
 
 #include "btBulletDynamicsCommon.h"
 
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
 class btBroadphaseInterface;
 class btCollisionShape;
 class btOverlappingPairCache;
@@ -191,7 +191,7 @@ class NNWalker
 	btRigidBody*		m_bodies[BODYPART_COUNT];
 	btTransform			m_bodyRelativeTransforms[BODYPART_COUNT];
 	btTypedConstraint*	m_joints[JOINT_COUNT];
-	std::map<void*,int>	m_bodyTouchSensorIndexMap;
+	btHashMap<btHashPtr,int>	m_bodyTouchSensorIndexMap;
 	bool 				m_touchSensors[BODYPART_COUNT];
 	btScalar			m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
 
@@ -269,7 +269,7 @@ public:
 		m_ownerWorld->addRigidBody(m_bodies[0]);
 		m_bodyRelativeTransforms[0] = btTransform::getIdentity();
 		m_bodies[0]->setUserPointer(this);
-		m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[0], 0));
+		m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0);
 
 		btHingeConstraint* hingeC;
 		//btConeTwistConstraint* coneC;
@@ -294,7 +294,7 @@ public:
 			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
 			m_bodyRelativeTransforms[1+2*i] = transform;
 			m_bodies[1+2*i]->setUserPointer(this);
-			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[1+2*i],1+2*i));
+			m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[1+2*i]),1+2*i);
 
 			// shin
 			transform.setIdentity();
@@ -302,7 +302,7 @@ public:
 			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
 			m_bodyRelativeTransforms[2+2*i] = transform;
 			m_bodies[2+2*i]->setUserPointer(this);
-			m_bodyTouchSensorIndexMap.insert(std::pair<void*,int>(m_bodies[2+2*i],2+2*i));
+			m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[2+2*i]),2+2*i);
 
 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
@@ -383,7 +383,7 @@ public:
 	}
 
 	void setTouchSensor(void* bodyPointer){
-		m_touchSensors[m_bodyTouchSensorIndexMap.at(bodyPointer)] = true;
+		m_touchSensors[*m_bodyTouchSensorIndexMap.find(btHashPtr(bodyPointer))] = true;
 	}
 
 	void clearTouchSensors(){