diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..80612b65d --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +/bin +/build3/gmake +/build_cmake/ diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp index 420ef7e22..0465998d2 100755 --- a/examples/Evolution/NN3DWalkers.cpp +++ b/examples/Evolution/NN3DWalkers.cpp @@ -20,6 +20,7 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btHashMap.h" + class btBroadphaseInterface; class btCollisionShape; class btOverlappingPairCache; @@ -28,22 +29,93 @@ class btConstraintSolver; struct btCollisionAlgorithmCreateFunc; class btDefaultCollisionConfiguration; class NNWalker; +class NNWalkerBrain; #include "NN3DWalkersTimeWarpBase.h" #include "../CommonInterfaces/CommonParameterInterface.h" #include "../Utils/b3ReferenceFrameHelper.hpp" +#include "../Utils/b3Clock.h" #include "../RenderingExamples/TimeSeriesCanvas.h" +// #### configurable parameters #### +#ifndef NUM_WALKER_LEGS +#define NUM_WALKER_LEGS 6 // the number of walker legs +#endif + +#ifndef POPULATION_SIZE +#define POPULATION_SIZE 10 // number of walkers in the population +#endif + +#ifndef EVALUATION_DURATION +#define EVALUATION_DURATION 10 // s (duration of one single evaluation) +#endif + +#ifndef TIME_SERIES_MAX_Y +#define TIME_SERIES_MAX_Y 20.0f // chart maximum y +#endif + +#ifndef TIME_SERIES_MIN_Y +#define TIME_SERIES_MIN_Y 0.0f // chart minimum y +#endif + +// walker dimensions and properties +static btScalar gWalkerMotorStrength = 0.5f; +static btScalar gWalkerLegTargetFrequency = 3; 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; - static btScalar gParallelEvaluations = 10.0f; +// Evaluation configurable parameters +#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 // number of walkers recreated via crossover +#endif + +// this means the rest of them is randomly created: REAP_QTY-SOW_CROSSOVER_QTY = NEW_RANDOM_BREED_QTY + +#ifndef SOW_ELITE_QTY +#define SOW_ELITE_QTY 0.2f // number of walkers kept using an elitist strategy (the best performing creatures are NOT mutated at all) +#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 // the chance an elite partner is chosen for breeding +#endif + +// #### debugging #### +#ifndef DRAW_INTERPENETRATIONS +#define DRAW_INTERPENETRATIONS false // DEBUG toggle: draw interpenetrations of a walker body +#endif + +#ifndef REBUILD_WALKER +#define REBUILD_WALKER true // if the walker should be rebuilt on mutation +#endif + +#ifndef TIMESTAMP_TIME +#define TIMESTAMP_TIME 2000.0f // delay between speed up timestamps +#endif + +// #### not to be reconfigured #### +#define BODYPART_COUNT (2 * NUM_WALKER_LEGS + 1) +#define JOINT_COUNT (BODYPART_COUNT - 1) + +void* GROUND_ID = (void*)1; + #ifndef SIMD_PI_4 #define SIMD_PI_4 0.5 * SIMD_HALF_PI #endif @@ -52,96 +124,91 @@ static btScalar gParallelEvaluations = 10.0f; #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 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 NN3DWalkersTimeWarpBase { - btScalar m_Time; - btScalar m_SpeedupTimestamp; - btScalar m_targetAccumulator; - btScalar m_targetFrequency; - btScalar m_motorStrength; - int m_evaluationsQty; - int m_nextReaped; - - btAlignedObjectArray m_walkersInPopulation; - - TimeSeriesCanvas* m_timeSeriesCanvas; + btScalar m_SimulationTime; // the current simulation time + btScalar m_LastSpeedupPrintTimestamp; + btScalar m_bestWalkerFitness; // to keep track of the best fitness + btVector3 m_resetPosition; // initial position of an evaluation + + int m_walkersInEvaluation; // number of walkers in evaluation + int m_nextReapedIndex; // index of the next reaped walker + + btAlignedObjectArray m_walkersInPopulation; // walkers in the population + btAlignedObjectArray m_walkerBrains; // walker cores in the population + + bool m_rebuildWorldNeeded; // if the world should be rebuilt (for determinism) + + btRigidBody* m_ground; // reference to ground to readd if world is rebuilt + + btOverlapFilterCallback * m_filterCallback; // the collision filter callback + + TimeSeriesCanvas* m_timeSeriesCanvas; // A plotting canvas for the walker fitnesses public: NN3DWalkersExample(struct GUIHelperInterface* helper) :NN3DWalkersTimeWarpBase(helper), - m_Time(0), - m_SpeedupTimestamp(0), - m_targetAccumulator(0), - m_targetFrequency(3), - m_motorStrength(0.5f), - m_evaluationsQty(0), - m_nextReaped(0), - m_timeSeriesCanvas(0) + // others + m_resetPosition(0,0,0), + m_SimulationTime(0), + m_bestWalkerFitness(0), + m_LastSpeedupPrintTimestamp(0), + m_walkersInEvaluation(0), + m_nextReapedIndex(0), + m_timeSeriesCanvas(NULL), + m_ground(NULL), + m_filterCallback(NULL) { + b3Clock clock; + clock.reset(true); // Reset clock to zero to get new random seed + srand(clock.getTimeMilliseconds()); // Set random milliseconds based on system time } virtual ~NN3DWalkersExample() { + //m_walkersInPopulation deallocates itself + //m_walkerBrains deallocates itself delete m_timeSeriesCanvas; } + /** + * Setup physics scene. + */ void initPhysics(); + /** + * Recreate the world if necessary. + * @param deltaTime + */ + virtual void performModelUpdate(float deltaTime); + + /** + * Delete the world and recreate it anew. + */ + void recreateWorld(); + + /** + * Shutdown physics scene. + */ virtual void exitPhysics(); - void spawnWalker(int index, const btVector3& startOffset, bool bFixed); - - virtual bool keyboardCallback(int key, int state); + /** + * Handle keyboard inputs. + * @param key + * @param state + * @return + */ + virtual bool keyboardCallback(int key, int state); + /** + * Detect collisions within simulation. Used to avoid collisions happening at startup. + * @return + */ bool detectCollisions(); + /** + * Reset the camera to a certain position and orientation. + */ void resetCamera() { float dist = 11; @@ -153,54 +220,141 @@ public: // Evaluation + /** + * Update the simulation. + * @param timeSinceLastTick + */ void update(const btScalar timeSinceLastTick); + /** + * Update all evaluations. + * @param timeSinceLastTick + */ void updateEvaluations(const btScalar timeSinceLastTick); + /** + * Schedule new evaluations and tear down old ones. + */ void scheduleEvaluations(); + /** + * Draw distance markings on ground. + */ void drawMarkings(); + /** + * Reset a walker by deleting and rebuilding it. + * @param i + * @param resetPosition + */ + void resetWalkerAt(int i, const btVector3& resetPosition); + // Reaper + /** + * Rate all evaluations via fitness function. + */ void rateEvaluations(); + /** + * Reap the worst performing walkers. + */ void reap(); + /** + * Sow new walkers. + */ void sow(); + /** + * Crossover two walkers to create an offspring. + * @param mother + * @param father + * @param offspring + */ void crossover(NNWalker* mother, NNWalker* father, NNWalker* offspring); + /** + * Mutate a walker. + * @param mutant + * @param mutationRate + */ void mutate(NNWalker* mutant, btScalar mutationRate); + /** + * Get a random elite walker. + * @return + */ NNWalker* getRandomElite(); + /** + * Get a random non elite walker. + * @return + */ NNWalker* getRandomNonElite(); + /** + * Get the next walker to be reaped. + * @return + */ NNWalker* getNextReaped(); + /** + * Print walker configurations to console. + */ void printWalkerConfigs(); - }; static NN3DWalkersExample* nn3DWalkers = NULL; +/** + * Neural Network Brain Weights. + */ +class NNWalkerBrain +{ + btScalar m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT]; // Neural Network connection weights + +public: + + NNWalkerBrain(){ + randomizeSensoryMotorWeights(); + } + + 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; + } + } + } + + btScalar* getSensoryMotorWeights() { + return m_sensoryMotorWeights; + } +}; + +/** + * Neural Networks Walker. A 6 legged robot moving controlled by a one-layer neural network which maps from leg ground touch sensors to leg joint positions. + */ 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]; + btDynamicsWorld* m_ownerWorld; + btCollisionShape* m_shapes[BODYPART_COUNT]; + btRigidBody* m_bodies[BODYPART_COUNT]; + btTransform m_bodyRelativeTransforms[BODYPART_COUNT]; + btTypedConstraint* m_joints[JOINT_COUNT]; btHashMap m_bodyTouchSensorIndexMap; - bool m_touchSensors[BODYPART_COUNT]; - btScalar m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT]; + bool m_touchSensors[BODYPART_COUNT]; + NNWalkerBrain* m_brain; - bool m_inEvaluation; - btScalar m_evaluationTime; - bool m_reaped; - btVector3 m_startPosition; - int m_index; + + bool m_inEvaluation; + btScalar m_evaluationTime; + bool m_reaped; + btVector3 m_startPosition; + int m_index; + btScalar m_legUpdateAccumulator; btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) { @@ -220,76 +374,77 @@ class NNWalker public: - 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(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed) - : m_ownerWorld (ownerWorld), - m_inEvaluation(false), - m_evaluationTime(0), - m_reaped(false) + NNWalker(int index, + btDynamicsWorld* ownerWorld, + NNWalkerBrain* core, + const btVector3& startingPosition, + const btScalar& rootBodyRadius, + const btScalar& rootBodyHeight, + const btScalar& legRadius, + const btScalar& legLength, + const btScalar& foreLegRadius, + const btScalar& foreLegLength, + bool fixedBodyPosition) + : m_ownerWorld(ownerWorld), // the world the walker walks in + m_brain(core), // the evolution core + m_inEvaluation(false), // the walker is not in evaluation + m_evaluationTime(0), // reset evaluation time + m_reaped(false), // the walker is not reaped + m_startPosition(startingPosition), // the starting position of the walker + m_legUpdateAccumulator(0), // variable to apply rhythmic leg position updates + m_index(index) { - m_index = index; - btVector3 vUp(0, 1, 0); // up in local reference frame + btVector3 vUp(0, 1, 0); // up direction in local reference frame NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo(); - randomizeSensoryMotorWeights(); - - // // Setup geometry - m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule + m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); // torso capsule int i; - for ( i=0; iaddRigidBody(m_bodies[0]); m_bodyRelativeTransforms[0] = btTransform::getIdentity(); m_bodies[0]->setUserPointer(this); m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0); btHingeConstraint* hingeC; - //btConeTwistConstraint* coneC; btTransform localA, localB, localC; // legs - for (i = 0; i < NUM_LEGS; i++) + for (i = 0; i < NUM_WALKER_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 - float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle + float footAngle = 2 * SIMD_PI * i / NUM_WALKER_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*(gRootBodyRadius+0.5*gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition*(gRootBodyRadius+0.5*gLegLength))); + btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(rootBodyRadius+0.5*legLength)), + btScalar(torsoAboveGroundHeight), + btScalar(footYUnitPosition*(rootBodyRadius+0.5*legLength))); transform.setOrigin(legCOM); // thigh - btVector3 legDirection = (legCOM - localRootBodyPosition).normalize(); + btVector3 legDirection = (legCOM - localTorsoPosition).normalize(); 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]); @@ -299,7 +454,9 @@ public: // shin transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength)))); + transform.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), + btScalar(torsoAboveGroundHeight-0.5*foreLegLength), + btScalar(footYUnitPosition*(rootBodyRadius+legLength)))); 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); @@ -307,39 +464,56 @@ public: // hip joints localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,-footAngle,0); localA.setOrigin(btVector3(btScalar(footXUnitPosition*gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*gRootBodyRadius))); + localA.getBasis().setEulerZYX(0,-footAngle,0); localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius), + btScalar(0.), + btScalar(footYUnitPosition*rootBodyRadius))); 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)); m_joints[2*i] = hingeC; // 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)))); + localA.getBasis().setEulerZYX(0,-footAngle,0); localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), + btScalar(0.), + btScalar(footYUnitPosition*(rootBodyRadius+legLength)))); 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)); m_joints[1+2*i] = hingeC; - m_ownerWorld->addRigidBody(m_bodies[1+2*i]); // add thigh bone + //test if we cause a collision with priorly inserted bodies. This prevents the walkers from having to resolve collisions on startup - m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root + 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(nnWalkersDemo->detectCollisions()){ // if thigh bone causes collision, remove it again + m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root + delete m_joints[2*i]; + m_joints[2*i] = NULL; - 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 + delete m_bodies[1+2*i]; + m_bodies[1+2*i] = NULL; + + delete m_joints[1+2*i]; + m_joints[1+2*i] = NULL; + delete m_bodies[2+2*i]; + m_bodies[2+2*i] = NULL; } 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 thigh - m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone - m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thigh + if(nnWalkersDemo->detectCollisions()){ // if shin bone causes collision, remove it again + m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh + delete m_joints[1+2*i]; + m_joints[1+2*i] = NULL; - 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 + delete m_bodies[2+2*i]; + m_bodies[2+2*i] = NULL; } } } @@ -347,36 +521,49 @@ public: // 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); + if(m_bodies[i] != NULL){ + m_bodies[i]->setDamping(0.05, 0.85); + m_bodies[i]->setDeactivationTime(0.8); + m_bodies[i]->setSleepingThresholds(0.5f, 0.5f); + m_bodies[i]->setActivationState(DISABLE_DEACTIVATION); + } } - removeFromWorld(); // it should not yet be in the world + // Properly remove rigidbodies and joints + removeJoints(); + removeRigidbodies(); + + clearTouchSensors(); // set touch sensors to zero + + randomizeSensoryMotorWeights(); // set random sensory motor weights for neural network layer } virtual ~NNWalker () { int i; + removeFromWorld(); // remove walker from world + // Remove all constraints for ( i = 0; i < JOINT_COUNT; ++i) { - m_ownerWorld->removeConstraint(m_joints[i]); - delete m_joints[i]; m_joints[i] = 0; + if(m_joints[i] != NULL){ + 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(); + if(m_bodies[i] != NULL){ + delete m_bodies[i]->getMotionState(); - delete m_bodies[i]; m_bodies[i] = 0; - delete m_shapes[i]; m_shapes[i] = 0; + delete m_bodies[i]; m_bodies[i] = 0; + delete m_shapes[i]; m_shapes[i] = 0; + } } + + m_ownerWorld = NULL; } btTypedConstraint** getJoints() { @@ -393,55 +580,47 @@ public: } } - bool getTouchSensor(int i){ - return m_touchSensors[i]; + btScalar* getSensoryMotorWeights() { + return m_brain->getSensoryMotorWeights(); } - btScalar* getSensoryMotorWeights() { - return m_sensoryMotorWeights; + void randomizeSensoryMotorWeights(){ + m_brain->randomizeSensoryMotorWeights(); } void addToWorld() { - int i; - // add all bodies and shapes - for ( i = 0; i < BODYPART_COUNT; ++i) - { - m_ownerWorld->addRigidBody(m_bodies[i]); - } + if(!isInEvaluation()){ - // 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 + addRigidbodies(); + addJoints(); + + setInEvaluation(true); } - m_startPosition = getPosition(); } - void removeFromWorld(){ - int i; + void removeFromWorld() { + if(isInEvaluation()){ - // Remove all constraints - for ( i = 0; i < JOINT_COUNT; ++i) - { - m_ownerWorld->removeConstraint(m_joints[i]); - } + removeJoints(); + removeRigidbodies(); - // Remove all bodies - for ( i = 0; i < BODYPART_COUNT; ++i) - { - m_ownerWorld->removeRigidBody(m_bodies[i]); + setInEvaluation(false); } } btVector3 getPosition() const { btVector3 finalPosition(0,0,0); + int j = 0; for(int i = 0; i < BODYPART_COUNT;i++) { - finalPosition += m_bodies[i]->getCenterOfMassPosition(); + if(m_bodies[i] != NULL){ + finalPosition += m_bodies[i]->getCenterOfMassPosition(); + j++; + } } - finalPosition /= BODYPART_COUNT; + finalPosition /= btScalar(j); return finalPosition; } @@ -459,20 +638,36 @@ public: return getDistanceFitness(); // for now it is only distance } + /** + * Reset walker to a position. Does not work deterministically. + * @param position + */ void resetAt(const btVector3& position) { + removeFromWorld(); btTransform resetPosition(btQuaternion::getIdentity(), position); + + for (int i = 0; i < JOINT_COUNT; i++) + { + btHingeConstraint* hingeC = static_cast(getJoints()[i]); + hingeC->enableAngularMotor(false,0,0); + } + 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)); + m_bodies[i]->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]); + if(m_bodies[i]->getMotionState()){ + m_bodies[i]->getMotionState()->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]); + } } + m_startPosition = position; // the starting position of the walker + + m_legUpdateAccumulator = 0; + clearTouchSensors(); } @@ -503,10 +698,82 @@ public: int getIndex() const { return m_index; } + + bool getTouchSensor(int i){ + return m_touchSensors[i]; + } + + NNWalkerBrain* getBrain(){ + return m_brain; + } + + btRigidBody** getBodies(){ + return &m_bodies[0]; + } + + btScalar getLegUpdateAccumulator() const { + return m_legUpdateAccumulator; + } + + void setLegUpdateAccumulator(btScalar legUpdateAccumulator) { + m_legUpdateAccumulator = legUpdateAccumulator; + } + + void setOwnerWorld(btDynamicsWorld* ownerWorld) { + m_ownerWorld = ownerWorld; + } + +private: + void addJoints(){ + // add all constraints + for (int i = 0; i < JOINT_COUNT; ++i) + { + if(m_joints[i] != NULL){ + m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies + } + } + } + + void addRigidbodies() { + // add all bodies + for (int i = 0; i < BODYPART_COUNT; ++i) + { + if(m_bodies[i] != NULL){ + m_ownerWorld->addRigidBody(m_bodies[i]); + } + } + } + + void removeJoints() { + // remove all constraints + for (int i = 0; i < JOINT_COUNT; ++i) + { + if(m_joints[i] != NULL){ + m_ownerWorld->removeConstraint(m_joints[i]); + } + } + } + + void removeRigidbodies() { + // remove all bodies + for (int i = 0; i < BODYPART_COUNT; ++i) + { + if(m_bodies[i] != NULL){ + m_ownerWorld->removeRigidBody(m_bodies[i]); + } + } + } }; void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep); +/** + * Draw leg contacts on ground. + * @param cp + * @param body0 + * @param body1 + * @return + */ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1) { btCollisionObject* o1 = static_cast(body0); @@ -515,27 +782,28 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1) void* ID1 = o1->getUserPointer(); void* ID2 = o2->getUserPointer(); - if (ID1 != GROUND_ID || ID2 != GROUND_ID) { + if (ID1 != GROUND_ID || ID2 != GROUND_ID) { // if one of the contacts is not ground // 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->mIsHeadless){ - nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.)); - } + if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL && !nn3DWalkers->mIsHeadless){ + nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.)); } - if(ID1 != GROUND_ID && ID1){ + if(ID1 != GROUND_ID && ID1){ // if ID1 is not ground ((NNWalker*)ID1)->setTouchSensor(o1); } - if(ID2 != GROUND_ID && ID2){ + if(ID2 != GROUND_ID && ID2){ // if ID2 is not ground ((NNWalker*)ID2)->setTouchSensor(o2); } } return false; } -struct WalkerFilterCallback : public btOverlapFilterCallback +/** + * A filter avoiding collision between walkers. + */ +struct WalkerFilterCallback : public btOverlapFilterCallback // avoids collisions between walkers { // return true when pairs need collision virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const @@ -546,9 +814,10 @@ struct WalkerFilterCallback : public btOverlapFilterCallback if (obj0->getUserPointer() == GROUND_ID || obj1->getUserPointer() == GROUND_ID) { // everything collides with ground return true; } - else{ + else if((NNWalker*)obj0->getUserPointer() && (NNWalker*)obj1->getUserPointer()){ return ((NNWalker*)obj0->getUserPointer())->getIndex() == ((NNWalker*)obj1->getUserPointer())->getIndex(); } + return true; } }; @@ -563,36 +832,44 @@ void NN3DWalkersExample::initPhysics() // Setup the basic world - m_Time = 0; + m_SimulationTime = 0; - createEmptyDynamicsWorld(); + gWalkerLegTargetFrequency = 3; // Hz - m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_walkersInPopulation.resize(POPULATION_SIZE, NULL); + m_walkerBrains.resize(POPULATION_SIZE, NULL); - m_targetFrequency = 3; + recreateWorld(); // setup world and add ground + + { // 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)); + m_ground = createRigidBody(btScalar(0.),groundTransform,groundShape); + m_ground->setFriction(5); + m_ground->setUserPointer(GROUND_ID); + } // 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; - + gWalkerMotorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations; { // create a slider to change the motor update frequency - SliderParams slider("Motor update frequency", &m_targetFrequency); + SliderParams slider("Motor update frequency", &gWalkerLegTargetFrequency); slider.m_minVal = 0; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the motor torque - SliderParams slider("Motor force", &m_motorStrength); + SliderParams slider("Motor force", &gWalkerMotorStrength); slider.m_minVal = 1; slider.m_maxVal = 50; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the root body radius @@ -600,8 +877,7 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the root body height @@ -609,8 +885,7 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the leg radius @@ -618,8 +893,7 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the leg length @@ -627,8 +901,7 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the fore leg radius @@ -636,8 +909,7 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a slider to change the fore leg length @@ -645,101 +917,135 @@ void NN3DWalkersExample::initPhysics() slider.m_minVal = 0.01f; slider.m_maxVal = 10; slider.m_clampToNotches = false; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter( - slider); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + if(POPULATION_SIZE > 1) { // 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_clampToIntegers = true; + slider.m_maxVal = POPULATION_SIZE; + slider.m_clampToIntegers = true; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } - - // 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)); - btRigidBody* ground = createRigidBody(btScalar(0.),groundTransform,groundShape); - ground->setFriction(5); - ground->setUserPointer(GROUND_ID); - } - - for(int i = 0; i < NUM_WALKERS ; i++){ - if(RANDOMIZE_DIMENSIONS){ - float maxDimension = 0.2f; - - // 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 offset(0,0,0); - 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++){ - m_timeSeriesCanvas->addDataSource(" ", 100*i/NUM_WALKERS,100*(NUM_WALKERS-i)/NUM_WALKERS,100*(i)/NUM_WALKERS); + // setup data sources for walkers in time series canvas + m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,400,300, "Fitness Performance"); + m_timeSeriesCanvas->setupTimeSeries(TIME_SERIES_MIN_Y, TIME_SERIES_MAX_Y, 10, 0); + for(int i = 0; i < POPULATION_SIZE ; i++){ + m_timeSeriesCanvas->addDataSource(" ", 100*i/POPULATION_SIZE, 100*(POPULATION_SIZE-i)/POPULATION_SIZE, 100*i/POPULATION_SIZE); } } +void NN3DWalkersExample::performModelUpdate(float deltaTime){ + if(m_rebuildWorldNeeded){ // rebuilding the world must be performed in sync to the update cycle of the example browser + recreateWorld(); + m_rebuildWorldNeeded = false; + } +} -void NN3DWalkersExample::spawnWalker(int index, const btVector3& startOffset, bool bFixed) -{ - NNWalker* walker = new NNWalker(index, m_dynamicsWorld, startOffset, bFixed); - m_walkersInPopulation.push_back(walker); +/** + * This method should really delete all remainings of the simulation except for the walker brains. + */ +void NN3DWalkersExample::recreateWorld(){ + + for(int i = 0; i < POPULATION_SIZE ; i++){ // remove and delete walkers + if(m_walkersInPopulation[i] != NULL){ + m_walkersInPopulation[i]->removeFromWorld(); + + for(int j = 0;j < BODYPART_COUNT;j++){ // remove all body graphics + btRigidBody* rb = static_cast(m_walkersInPopulation[i]->getBodies()[j]); + if (rb != NULL) { + int graphicsUid = rb->getUserIndex(); + m_guiHelper->removeGraphicsInstance(graphicsUid); + } + } + + delete m_walkersInPopulation[i]; + m_walkersInPopulation[i] = NULL; + } + } + + if(m_dynamicsWorld != NULL && m_ground != NULL){ + m_dynamicsWorld->removeRigidBody(m_ground); // remove ground + } + + // delete world etc. + if(m_dynamicsWorld != NULL){ + delete m_dynamicsWorld; + m_dynamicsWorld = 0; + } + + if(m_solver != NULL){ + delete m_solver; + m_solver = 0; + } + + if(m_broadphase != NULL){ + delete m_broadphase; + m_broadphase = 0; + } + + if(m_dispatcher != NULL){ + delete m_dispatcher; + m_dispatcher = 0; + } + + if(m_collisionConfiguration != NULL){ + delete m_collisionConfiguration; + m_collisionConfiguration = 0; + } + + createEmptyDynamicsWorld(); // create new world + + // add all filters and callbacks + m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); // set evolution update pretick callback + m_filterCallback = new WalkerFilterCallback(); + m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback); // avoid collisions between walkers + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode(0); + + if(m_dynamicsWorld != NULL && m_ground != NULL){ + m_dynamicsWorld->addRigidBody(m_ground); // readd ground + } } bool NN3DWalkersExample::detectCollisions() { bool collisionDetected = false; - if(m_dynamicsWorld){ - m_dynamicsWorld->performDiscreteCollisionDetection(); // let the collisions be calculated - } + 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(); + int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds(); + for (int i = 0; i < numManifolds; i++) // for each collision + { + btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); + // get collision objects of collision + const btCollisionObject* obA = contactManifold->getBody0(); + const btCollisionObject* obB = contactManifold->getBody1(); - if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){ + if (obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID && obA->getUserPointer() == obB->getUserPointer()) { // if one of them is not ground - int numContacts = contactManifold->getNumContacts(); - for (int j=0;jgetContactPoint(j); - if (pt.getDistance()<0.f) + int numContacts = contactManifold->getNumContacts(); + for (int j = 0; j < numContacts; j++) // for each collision contact { - //const btVector3& ptA = pt.getPositionWorldOnA(); - //const btVector3& ptB = pt.getPositionWorldOnB(); - //const btVector3& normalOnB = pt.m_normalWorldOnB; + collisionDetected = true; + btManifoldPoint& pt = contactManifold->getContactPoint(j); + if (pt.getDistance() < 0.f) // if collision is penetrating + { + const btVector3& ptA = pt.getPositionWorldOnA(); + const btVector3& ptB = pt.getPositionWorldOnB(); + const btVector3& normalOnB = pt.m_normalWorldOnB; - if(!DRAW_INTERPENETRATIONS){ - return collisionDetected; - } + if (!DRAW_INTERPENETRATIONS) { // if no interpenetrations are drawn, break + break; + } - 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.)); + if (m_dynamicsWorld->getDebugDrawer() && !mIsHeadless) { // draw self collisions + m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.)); + m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.)); + } } } } @@ -754,13 +1060,13 @@ bool NN3DWalkersExample::keyboardCallback(int key, int state) switch (key) { case '[': - m_motorStrength /= 1.1f; + gWalkerMotorStrength /= 1.1f; return true; case ']': - m_motorStrength *= 1.1f; + gWalkerMotorStrength *= 1.1f; return true; case 'l': - printWalkerConfigs(); +// printWalkerConfigs(); return true; default: break; @@ -774,9 +1080,7 @@ void NN3DWalkersExample::exitPhysics() gContactProcessedCallback = NULL; // clear contact processed callback on exiting - int i; - - for (i = 0;i < NUM_WALKERS;i++) + for (int i = 0;i < POPULATION_SIZE;i++) { NNWalker* walker = m_walkersInPopulation[i]; delete walker; @@ -802,41 +1106,61 @@ void NN3DWalkersExample::rateEvaluations(){ b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness())); - for(int i = 0; i < NUM_WALKERS;i++){ - m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),0,true); + // if not all walkers are reaped and the best walker is worse than is had been in the previous round + if((POPULATION_SIZE-1)*(1-REAP_QTY) != 0 && btSqrt(m_walkersInPopulation[0]->getDistanceFitness()) < m_bestWalkerFitness){ + b3Printf("################Simulation not deterministic###########################"); + } + else{ + m_bestWalkerFitness = btSqrt(m_walkersInPopulation[0]->getDistanceFitness()); } - m_timeSeriesCanvas->nextTick(); - for(int i = 0; i < NUM_WALKERS;i++){ + for(int i = 0; i < POPULATION_SIZE;i++){ // plot walker fitnesses for this round + m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),i,true); + } + m_timeSeriesCanvas->nextTick(); // move to next tick of graph + + for(int i = 0; i < POPULATION_SIZE;i++){ // reset all walkers m_walkersInPopulation[i]->setEvaluationTime(0); } - m_nextReaped = 0; + m_nextReapedIndex = 0; } void NN3DWalkersExample::reap() { int reaped = 0; - for(int i = NUM_WALKERS-1;i >=(NUM_WALKERS-1)*(1-REAP_QTY); i--){ // reap a certain percentage + for(int i = POPULATION_SIZE-1;i >=(POPULATION_SIZE-1)*(1-REAP_QTY); i--){ // reap a certain percentage of walkers to replace them afterwards m_walkersInPopulation[i]->setReaped(true); reaped++; - b3Printf("%i Walker(s) reaped.",reaped); } + b3Printf("%i Walker(s) reaped.",reaped); } +/** + * Return a random elitist walker (one that is not mutated at all because it performs well). + * @return Random elitist walker. + */ NNWalker* NN3DWalkersExample::getRandomElite(){ - return m_walkersInPopulation[((NUM_WALKERS-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)]; + return m_walkersInPopulation[((POPULATION_SIZE-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)]; } +/** + * Return a random non-elitist walker (a mutated walker). + * @return + */ NNWalker* NN3DWalkersExample::getRandomNonElite(){ - return m_walkersInPopulation[(NUM_WALKERS-1) * SOW_ELITE_QTY + (NUM_WALKERS-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)]; + return m_walkersInPopulation[(POPULATION_SIZE-1) * SOW_ELITE_QTY + (POPULATION_SIZE-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)]; } +/** + * Get the next reaped walker to be replaced. + * @return + */ NNWalker* NN3DWalkersExample::getNextReaped() { - if((NUM_WALKERS-1) - m_nextReaped >= (NUM_WALKERS-1) * (1-REAP_QTY)){ - m_nextReaped++; + if((POPULATION_SIZE-1) - m_nextReapedIndex >= (POPULATION_SIZE-1) * (1-REAP_QTY)){ + m_nextReapedIndex++; } - if(m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1]->isReaped()){ - return m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1]; + if(m_walkersInPopulation[(POPULATION_SIZE-1) - m_nextReapedIndex+1]->isReaped()){ + return m_walkersInPopulation[(POPULATION_SIZE-1) - m_nextReapedIndex+1]; } else{ return NULL; // we asked for too many @@ -844,30 +1168,38 @@ NNWalker* NN3DWalkersExample::getNextReaped() { } +/** + * Sow new walkers. + */ void NN3DWalkersExample::sow() { int sow = 0; - for(int i = 0; i < NUM_WALKERS * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures + for(int i = 0; i < POPULATION_SIZE * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures sow++; - b3Printf("%i Walker(s) sown.",sow); 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 = 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 = POPULATION_SIZE*SOW_ELITE_QTY; i < POPULATION_SIZE*(SOW_ELITE_QTY+SOW_MUTATION_QTY);i++){ // create mutants + mutate(m_walkersInPopulation[i], btScalar(MUTATION_RATE / (POPULATION_SIZE * SOW_MUTATION_QTY) * (i-POPULATION_SIZE*SOW_ELITE_QTY))); } - for(int i = 0; i < (NUM_WALKERS-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){ + for(int i = 0; i < (POPULATION_SIZE-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){ sow++; - b3Printf("%i Walker(s) sown.",sow); NNWalker* reaped = getNextReaped(); reaped->setReaped(false); reaped->randomizeSensoryMotorWeights(); } + b3Printf("%i Walker(s) sown.",sow); } +/** + * Crossover mother and father into the child. + * @param mother + * @param father + * @param child + */ 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)); @@ -882,6 +1214,11 @@ void NN3DWalkersExample::crossover(NNWalker* mother, NNWalker* father, NNWalker* } } +/** + * Mutate the mutant. + * @param mutant + * @param mutationRate + */ void NN3DWalkersExample::mutate(NNWalker* mutant, btScalar mutationRate) { for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){ btScalar random = ((double) rand() / (RAND_MAX)); @@ -892,12 +1229,20 @@ void NN3DWalkersExample::mutate(NNWalker* mutant, btScalar mutationRate) { } } +/** + * Update the demo via pretick callback to be precise. + * @param world + * @param timeStep + */ void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep) { NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)world->getWorldUserInfo(); - nnWalkersDemo->update(timeStep); } +/** + * Update cycle. + * @param timeSinceLastTick + */ void NN3DWalkersExample::update(const btScalar timeSinceLastTick) { updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */ @@ -905,12 +1250,16 @@ void NN3DWalkersExample::update(const btScalar timeSinceLastTick) { drawMarkings(); /**!< Draw markings on the ground */ - if(m_Time > m_SpeedupTimestamp + 2.0f){ // print effective speedup + if(m_SimulationTime > m_LastSpeedupPrintTimestamp + TIMESTAMP_TIME){ // print effective speedup every x seconds b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup()); - m_SpeedupTimestamp = m_Time; + m_LastSpeedupPrintTimestamp = m_SimulationTime; } } +/** + * Update the evaluations. + * @param timeSinceLastTick + */ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) { btScalar delta = timeSinceLastTick; btScalar minFPS = 1.f/60.f; @@ -918,141 +1267,185 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) { delta = minFPS; } - m_Time += delta; + m_SimulationTime += delta; - m_targetAccumulator += delta; - - for(int i = 0; i < NUM_WALKERS;i++) // evaluation time passes + for(int r = 0; r < POPULATION_SIZE;r++) // evaluation time passes { - if(m_walkersInPopulation[i]->isInEvaluation()){ - m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time - } - } + if(m_walkersInPopulation[r] != NULL && m_walkersInPopulation[r]->isInEvaluation()){ + m_walkersInPopulation[r]->setEvaluationTime(m_walkersInPopulation[r]->getEvaluationTime()+delta); // increase evaluation time - if(m_targetAccumulator >= 1.0f /((double)m_targetFrequency)) - { - m_targetAccumulator = 0; + m_walkersInPopulation[r]->setLegUpdateAccumulator(m_walkersInPopulation[r]->getLegUpdateAccumulator() + delta); - for (int r=0; risInEvaluation()) + if(m_walkersInPopulation[r]->getLegUpdateAccumulator() >= btScalar(1.0f) /gWalkerLegTargetFrequency) // we update the leg movement with leg target frequency { - for (int i = 0; i < 2*NUM_LEGS; i++) + m_walkersInPopulation[r]->setLegUpdateAccumulator(0); + + for (int i = 0; i < 2*NUM_WALKER_LEGS; i++) { - btScalar targetAngle = 0; + btScalar targetAngle = 0; // angle in range [0,1] btHingeConstraint* hingeC = static_cast(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++){ + if(hingeC != NULL){ // if hinge exists + for(int j = 0; j < JOINT_COUNT;j++){ // accumulate sensor inputs with weights (summate inputs) 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 = 0; - if(delta){ - desiredAngularVel = angleError/delta; - } - else{ - desiredAngularVel = angleError/0.0001f; - } - hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength); - } + targetAngle = (tanh(targetAngle)+1.0f)*0.5f; // apply the activation function (threshold) [0;1] - // clear sensor signals after usage - m_walkersInPopulation[r]->clearTouchSensors(); + btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); // [lowerLimit;upperLimit] + btScalar currentAngle = hingeC->getHingeAngle(); + btScalar angleError = targetLimitAngle - currentAngle; // target current delta + btScalar desiredAngularVel = angleError/((delta>0)?delta:btScalar(0.0001f)); // division by zero safety + + hingeC->enableAngularMotor(true, desiredAngularVel, gWalkerMotorStrength); // set new target velocity + } + } } + + // clear sensor signals after usage + m_walkersInPopulation[r]->clearTouchSensors(); } } } +/** + * Schedule the walker evaluations. + */ void NN3DWalkersExample::scheduleEvaluations() { - for(int i = 0; i < NUM_WALKERS;i++){ + for(int i = 0; i < POPULATION_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); + if(m_walkersInPopulation[i] != NULL && m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_DURATION){ /**!< tear down evaluations */ + b3Printf("An evaluation finished at %f s. Distance: %f m", m_SimulationTime, btSqrt(m_walkersInPopulation[i]->getDistanceFitness())); m_walkersInPopulation[i]->removeFromWorld(); - m_evaluationsQty--; + m_walkersInEvaluation--; } - 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); + if(m_walkersInEvaluation < gParallelEvaluations && (m_walkersInPopulation[i] == NULL || (!m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0))){ /**!< Setup the new evaluations */ + b3Printf("An evaluation started at %f s.", m_SimulationTime); + m_walkersInEvaluation++; - if(m_walkersInPopulation[i]->getEvaluationTime() == 0){ // reset to origin if the evaluation did not yet run - m_walkersInPopulation[i]->resetAt(btVector3(0,0,0)); + if(REBUILD_WALKER){ // deletes and recreates the walker in the position + m_guiHelper->removeAllGraphicsInstances(); + m_ground->setUserIndex(-1); // reset to get a new graphics object + m_ground->setUserIndex2(-1); // reset to get a new graphics object + m_ground->getCollisionShape()->setUserIndex(-1); // reset to get a new graphics object + + resetWalkerAt(i, m_resetPosition); + } + else{ // resets the position of the walker without deletion + m_walkersInPopulation[i]->resetAt(m_resetPosition); } - 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 + if(!mIsHeadless){ // after all changes, regenerate graphics objects + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + } - reap(); // reap worst performing walkers + if(m_walkersInEvaluation == 0){ // if there are no more evaluations possible + if(REBUILD_WALKER){ + m_rebuildWorldNeeded = true; + } - sow(); // crossover & mutate and sow new walkers + 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. ###"); } } +/** + * Reset the walker at position. + * @param i + * @param resetPosition + */ +void NN3DWalkersExample::resetWalkerAt(int i, const btVector3& resetPosition){ + + // either copy core or create a new one if no previous existing + m_walkerBrains[i] = (m_walkerBrains[i] != NULL)?m_walkerBrains[i]:new NNWalkerBrain(); + + NNWalker* newWalker = new NNWalker(i, + m_dynamicsWorld, + m_walkerBrains[i], + resetPosition, + gRootBodyRadius, + gRootBodyHeight, + gLegRadius, + gLegLength, + gForeLegRadius, + gForeLegLength, + false); + + if(m_walkersInPopulation[i] != NULL){ + delete m_walkersInPopulation[i]; + } + + m_walkersInPopulation[i] = newWalker; +} + +/** + * Draw distance markings on the ground. + */ void NN3DWalkersExample::drawMarkings() { - if(!mIsHeadless){ - for(int i = 0; i < NUM_WALKERS;i++) // draw current distance plates of moving walkers + if(!mIsHeadless){ // if the system is not running headless + for(int i = 0; i < POPULATION_SIZE;i++) // draw current distance plates of moving walkers { if(m_walkersInPopulation[i]->isInEvaluation()){ btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition(); char performance[20]; sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness())); - m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),1); + m_guiHelper->drawText3D(performance, + walkerPosition.x(), + walkerPosition.y() + 1, + walkerPosition.z(), + 1); } } - 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); + if(m_dynamicsWorld->getDebugDrawer()){ + for(int i = 2; i < 50; i += 2){ // draw distance circles + 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(){ -#if 0 - 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; - 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); -#endif -} +/** + * Print walker neural network layer configurations. +*/ +//void NN3DWalkersExample::printWalkerConfigs(){ +// char configString[25 + POPULATION_SIZE*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + POPULATION_SIZE*4 + 1]; // 15 precision + [],\n +// char* runner = configString; +// sprintf(runner,"Population configuration:"); +// runner +=25; +// for(int i = 0;i < POPULATION_SIZE;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); +//} diff --git a/examples/Evolution/NN3DWalkersTimeWarpBase.h b/examples/Evolution/NN3DWalkersTimeWarpBase.h index 0c97c3850..64bfbc885 100644 --- a/examples/Evolution/NN3DWalkersTimeWarpBase.h +++ b/examples/Evolution/NN3DWalkersTimeWarpBase.h @@ -208,7 +208,8 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { CommonRigidBodyBase(helper), mPhysicsStepsPerSecondUpdated(false), mFramesPerSecondUpdated(false), - mSolverIterationsUpdated(false) { + mSolverIterationsUpdated(false), + mIsHeadless(false){ // main frame timer initialization mApplicationStart = mLoopTimer.getTimeMilliseconds(); /**!< Initialize when the application started running */ @@ -519,7 +520,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { m_collisionConfiguration); } - changeERPCFM(); // set appropriate ERP/CFM values according to the string and damper properties of the constraint + changeERPCFM(); // set appropriate ERP/CFM values according to the spring 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 @@ -536,7 +537,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { 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 + m_dynamicsWorld->setGravity(btVector3(0, btScalar(-9.81f), 0)); // set gravity to -9.81 } @@ -551,7 +552,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { - void timeWarpSimulation(float deltaTime) // Override this + virtual void performModelUpdate(float deltaTime) // Override this { } @@ -559,7 +560,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { void stepSimulation(float deltaTime){ // customly step the simulation do{ -// // settings + // settings if(mPhysicsStepsPerSecondUpdated){ changePhysicsStepsPerSecond(gPhysicsStepsPerSecond); mPhysicsStepsPerSecondUpdated = false; @@ -590,10 +591,10 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { //############# // 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); + performModelUpdate(deltaTime); if(mLoopTimer.getTimeSeconds() - speedUpPrintTimeStamp > 1){ // on reset, we calculate the performed speed up - //double speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp)); + double speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp)); // b3Printf("Avg Effective speedup: %f",speedUp); performedTime = 0; performanceTimestamp = mLoopTimer.getTimeMilliseconds(); @@ -612,7 +613,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { 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*/) { + 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(); @@ -627,8 +628,8 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { 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 */ + //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 */ @@ -650,7 +651,7 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { } - virtual bool keyboardCallback(int key, int state) + virtual bool keyboardCallback(int key, int state) { switch(key) { @@ -752,10 +753,9 @@ struct NN3DWalkersTimeWarpBase: public CommonRigidBodyBase { 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; + int subSteps = 1; // since we want to perform all proper steps, we perform no interpolated substeps - m_dynamicsWorld->stepSimulation(btScalar(timeStep), + m_dynamicsWorld->stepSimulation(btScalar(fixedPhysicsStepSizeSec), btScalar(subSteps), btScalar(fixedPhysicsStepSizeSec)); } } diff --git a/examples/RenderingExamples/TimeSeriesCanvas.cpp b/examples/RenderingExamples/TimeSeriesCanvas.cpp index 096579839..ee4882d36 100644 --- a/examples/RenderingExamples/TimeSeriesCanvas.cpp +++ b/examples/RenderingExamples/TimeSeriesCanvas.cpp @@ -13,7 +13,11 @@ struct DataSource float m_lastValue; bool m_hasLastValue; DataSource() - :m_hasLastValue(false) + :m_red(0), + m_green(0), + m_blue(0), + m_lastValue(0), + m_hasLastValue(false) { } }; @@ -28,10 +32,12 @@ struct TimeSeriesInternalData int m_height; float m_pixelsPerUnit; - float m_zero; + float m_center; int m_timeTicks; int m_ticksPerSecond; - float m_yScale; + float m_yMax; + float m_yMin; + float m_yZero; int m_bar; unsigned char m_backgroundRed; @@ -50,13 +56,17 @@ struct TimeSeriesInternalData } TimeSeriesInternalData(int width, int height) - :m_width(width), + :m_canvasInterface(NULL), + m_canvasIndex(0), + m_width(width), m_height(height), m_pixelsPerUnit(-100), - m_zero(height/2.0), + m_center(height/2.0), m_timeTicks(0), m_ticksPerSecond(100), - m_yScale(1), + m_yMax(1), + m_yMin(0), + m_yZero(0.5f), m_bar(0), m_backgroundRed(255), m_backgroundGreen(255), @@ -105,13 +115,18 @@ void TimeSeriesCanvas::addDataSource(const char* dataSourceLabel, unsigned char } void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas) +{ + setupTimeSeries(-yScale, yScale, ticksPerSecond, startTime, clearCanvas); +} +void TimeSeriesCanvas::setupTimeSeries(float yMin, float yMax, int ticksPerSecond, int startTime, bool clearCanvas) { if (0==m_internalData->m_canvasInterface) return; - m_internalData->m_pixelsPerUnit = -(m_internalData->m_height/3.f)/yScale; + m_internalData->m_pixelsPerUnit = -(m_internalData->m_height/3.f)/(0.5f*(yMax-yMin)); m_internalData->m_ticksPerSecond = ticksPerSecond; - m_internalData->m_yScale = yScale; + m_internalData->m_yMin = yMin; + m_internalData->m_yMax = yMax; m_internalData->m_dataSources.clear(); if (clearCanvas) @@ -130,18 +145,22 @@ void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int sta } } - float zeroPixelCoord = m_internalData->m_zero; + float centerPixelCoord = m_internalData->m_center; float pixelsPerUnit = m_internalData->m_pixelsPerUnit; - float yPos = zeroPixelCoord+pixelsPerUnit*yScale; - float yNeg = zeroPixelCoord+pixelsPerUnit*-yScale; + m_internalData->m_yZero = centerPixelCoord - pixelsPerUnit*0.5f*(yMax+yMin); + + float yPos = centerPixelCoord+pixelsPerUnit*0.5f*(yMax-yMin); + float yNeg = centerPixelCoord+pixelsPerUnit*-0.5f*(yMax-yMin); - grapicalPrintf("0", sTimeSeriesFontData, 2,zeroPixelCoord,m_internalData->m_textColorRed,m_internalData->m_textColorGreen,m_internalData->m_textColorBlue,m_internalData->m_textColorAlpha); + if(yMin < 0 && yMax > 0){ + grapicalPrintf("0", sTimeSeriesFontData, 2,m_internalData->m_yZero,m_internalData->m_textColorRed,m_internalData->m_textColorGreen,m_internalData->m_textColorBlue,m_internalData->m_textColorAlpha); + } char label[1024]; - sprintf(label,"%2.1f", yScale); + sprintf(label,"%2.1f", yMax); grapicalPrintf(label, sTimeSeriesFontData, 2,yPos,m_internalData->m_textColorRed,m_internalData->m_textColorGreen,m_internalData->m_textColorBlue,m_internalData->m_textColorAlpha); - sprintf(label,"%2.1f", -yScale); + sprintf(label,"%2.1f", yMin); grapicalPrintf(label, sTimeSeriesFontData, 2,yNeg,m_internalData->m_textColorRed,m_internalData->m_textColorGreen,m_internalData->m_textColorBlue,m_internalData->m_textColorAlpha); m_internalData->m_canvasInterface->refreshImageData(m_internalData->m_canvasIndex); @@ -209,7 +228,7 @@ void TimeSeriesCanvas::shift1PixelToLeft() int resetVal = 10; int countdown = resetVal; - //shift pixture one pixel to the left + //shift picture one pixel to the left for (int j=50;jm_height-48;j++) { for (int i=40;im_internalData->m_width;i++) @@ -238,35 +257,34 @@ void TimeSeriesCanvas::shift1PixelToLeft() } } - + { + int resetVal = 2; + static int countdown = resetVal; + if (!countdown--) { - int resetVal = 2; - static int countdown = resetVal; - if (!countdown--) - { - countdown=resetVal; - m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1,m_internalData->m_zero,0,0,0,255); - } + countdown=resetVal; + m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1,m_internalData->m_yZero,0,0,0,255); } + } + { + int resetVal = 10; + static int countdown = resetVal; + if (!countdown--) { - int resetVal = 10; - static int countdown = resetVal; - if (!countdown--) - { - countdown=resetVal; - float zeroPixelCoord = m_internalData->m_zero; - float pixelsPerUnit = m_internalData->m_pixelsPerUnit; + countdown=resetVal; + float centerPixelCoord = m_internalData->m_center; + float pixelsPerUnit = m_internalData->m_pixelsPerUnit; - float yPos = zeroPixelCoord+pixelsPerUnit*m_internalData->m_yScale; - float yNeg = zeroPixelCoord+pixelsPerUnit*-m_internalData->m_yScale; + float yPos = centerPixelCoord+pixelsPerUnit*0.5f*(m_internalData->m_yMax-m_internalData->m_yMin); + float yNeg = centerPixelCoord+pixelsPerUnit*-0.5f*(m_internalData->m_yMax-m_internalData->m_yMin); - m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1, - yPos,0,0,0,255); - m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1, - yNeg,0,0,0,255); - } + m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1, + yPos,0,0,0,255); + m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,m_internalData->m_width-1, + yNeg,0,0,0,255); } + } @@ -276,7 +294,7 @@ void TimeSeriesCanvas::shift1PixelToLeft() char buf[1024]; float time = m_internalData->getTime(); sprintf(buf,"%2.0f",time); - grapicalPrintf(buf, sTimeSeriesFontData, m_internalData->m_width-25,m_internalData->m_zero+3,0,0,0,255); + grapicalPrintf(buf, sTimeSeriesFontData, m_internalData->m_width-25,m_internalData->m_center+3,0,0,0,255); m_internalData->m_bar=m_internalData->m_ticksPerSecond; @@ -294,7 +312,7 @@ void TimeSeriesCanvas::insertDataAtCurrentTime(float orgV, int dataSourceIndex, btAssert(dataSourceIndex < m_internalData->m_dataSources.size()); - float zero = m_internalData->m_zero; + float zero = m_internalData->m_yZero; float amp = m_internalData->m_pixelsPerUnit; //insert some new value(s) in the right-most column { diff --git a/examples/RenderingExamples/TimeSeriesCanvas.h b/examples/RenderingExamples/TimeSeriesCanvas.h index 41f466279..29901e8da 100644 --- a/examples/RenderingExamples/TimeSeriesCanvas.h +++ b/examples/RenderingExamples/TimeSeriesCanvas.h @@ -13,6 +13,7 @@ public: virtual ~TimeSeriesCanvas(); void setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas=true); + void setupTimeSeries(float yMin, float yMax, int ticksPerSecond, int startTime, bool clearCanvas=true); void addDataSource(const char* dataSourceLabel, unsigned char red,unsigned char green,unsigned char blue); void insertDataAtCurrentTime(float value, int dataSourceIndex, bool connectToPrevious); float getCurrentTime() const; @@ -22,4 +23,4 @@ public: }; -#endif//TIME_SERIES_CANVAS_H \ No newline at end of file +#endif//TIME_SERIES_CANVAS_H