test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/random.h>
#include <mrpt/system/os.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSphere.h>
#include <iostream>
 
using namespace mrpt;
using namespace std;
using namespace mrpt::gui;
using namespace mrpt::random;
using namespace mrpt::opengl;
 
const size_t N_MASSES = 750;
 
const double BOX = 500;
const double V0 = 100;
const double MASS_MIN = log(40.0), MASS_MAX = log(100.0);
const double M2R = 2.0;
const double LARGEST_STEP = 0.001;
const double G = 300;
const double COLLIS_LOSS = 0.98;
 
struct TMass
{
        TMass() : x(0), y(0), z(0), vx(0), vy(0), vz(0), mass(1), obj3d() {}
        double x, y, z;
        double vx, vy, vz;
        double mass;
        double radius;
        opengl::CSphere::Ptr obj3d;
};
 
void simulateGravity(vector<TMass>& objs, double At);
 
// ------------------------------------------------------
//                              GravityDemo
// ------------------------------------------------------
void GravityDemo()
{
        CDisplayWindow3D win(
                "MRPT example: 3D gravity simulator- JLBC 2008", 1000, 700);
 
        getRandomGenerator().randomize();
 
        win.setCameraElevationDeg(50.0f);
        win.setCameraZoom(1000);
 
        COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
 
        // Modify the scene:
        // ------------------------------------------------------
        {
                opengl::CGridPlaneXY::Ptr obj =
                        mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
                                -2000, 2000, -2000, 2000, 0, 100);
                obj->setColor(0.3, 0.3, 0.3);
                theScene->insert(obj);
        }
 
        // Create the masses:
        // ----------------------------------------------------
 
        vector<TMass> masses(N_MASSES);
 
        // Init at random poses & create opengl objects:
        for (size_t i = 0; i < N_MASSES; i++)
        {
                masses[i].x = getRandomGenerator().drawUniform(-BOX, BOX);
                masses[i].y = getRandomGenerator().drawUniform(-BOX, BOX);
                masses[i].z = getRandomGenerator().drawUniform(-BOX, BOX) / 10;
 
                double a = atan2(masses[i].y, masses[i].x);
 
                masses[i].vx = -V0 * sin(a) +
                                           getRandomGenerator().drawUniform(-V0 * 0.01, V0 * 0.01);
                masses[i].vy = V0 * cos(a) +
                                           getRandomGenerator().drawUniform(-V0 * 0.01, V0 * 0.01);
                masses[i].vz = 0;  // getRandomGenerator().drawUniform(-V0,V0);
 
                masses[i].mass =
                        exp(getRandomGenerator().drawUniform(MASS_MIN, MASS_MAX));
                opengl::CSphere::Ptr& obj = masses[i].obj3d =
                        mrpt::make_aligned_shared<opengl::CSphere>();
 
                obj->setColor(
                        getRandomGenerator().drawUniform(0.1, 0.9),
                        getRandomGenerator().drawUniform(0.1, 0.9),
                        getRandomGenerator().drawUniform(0.1, 0.9));
 
                masses[i].radius = M2R * pow(masses[i].mass, 1.0 / 3.0);
                obj->setRadius(masses[i].radius);  // Guess why ^(1/3) ;-)
 
                obj->setLocation(masses[i].x, masses[i].y, masses[i].z);
                theScene->insert(obj);
        }
 
        // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
        win.unlockAccess3DScene();
 
        mrpt::system::CTicTac tictac;
        tictac.Tic();
 
        double t0 = tictac.Tac();
 
        while (!mrpt::system::os::kbhit() && win.isOpen())
        {
                // Move the scene:
                double t1 = tictac.Tac();
                double At = t1 - t0;
                t0 = t1;
 
                // Simulate a At, possibly in several small steps:
                // Update the 3D scene:
                win.get3DSceneAndLock();
 
                size_t n_steps = ceil(At / LARGEST_STEP) + 1;
                double At_steps = At / n_steps;
                n_steps = min(n_steps, size_t(3));
                for (size_t j = 0; j < n_steps; j++) simulateGravity(masses, At_steps);
 
                for (size_t i = 0; i < masses.size(); i++)
                {
                        opengl::CSphere::Ptr& obj = masses[i].obj3d;
                        obj->setLocation(masses[i].x, masses[i].y, masses[i].z);
                }
                // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
                win.unlockAccess3DScene();
 
                // Update window:
                win.forceRepaint();
                std::this_thread::sleep_for(1ms);
        };
}
 
struct TForce
{
        TForce() { f[0] = f[1] = f[2] = 0; }
        double f[3];
};
 
void simulateGravity(vector<TMass>& objs, double At)
{
        const size_t N = objs.size();
 
        vector<TForce> forces(N);
 
        // Index in the array must be larger than its content!!
        vector<pair<size_t, double>> lstMass_i_joins_j(
                N, pair<size_t, double>(string::npos, 10000.0));
 
        for (size_t i = 0; i < (N - 1); i++)
        {
                const double Ri = objs[i].radius;
 
                // Compute overall gravity force:
                for (size_t j = i + 1; j < N; j++)
                {
                        double Ax = objs[j].x - objs[i].x;
                        double Ay = objs[j].y - objs[i].y;
                        double Az = objs[j].z - objs[i].z;
                        double D2 = square(Ax) + square(Ay) + square(Az);
 
                        double D = sqrt(D2);
                        if (D == 0) continue;
 
                        const double Rj = objs[j].radius;
 
                        if (D < (Ri + Rj))  // Collission!!
                        {
                                // Index in the array must be larger than its content!!
                                if (D < lstMass_i_joins_j[j].second)
                                {
                                        lstMass_i_joins_j[j].first = i;
                                        lstMass_i_joins_j[j].second = D;
                                }
                        }
                        else
                        {
                                double K =
                                        G * objs[i].mass * objs[j].mass / square(max(D, 1.0));
                                double D_1 = 1.0 / D;
                                Ax *= D_1;
                                Ay *= D_1;
                                Az *= D_1;
 
                                const double fx = Ax * K;
                                const double fy = Ay * K;
                                const double fz = Az * K;
 
                                forces[i].f[0] += fx;
                                forces[i].f[1] += fy;
                                forces[i].f[2] += fz;
 
                                forces[j].f[0] -= fx;
                                forces[j].f[1] -= fy;
                                forces[j].f[2] -= fz;
                        }
                }
        }
 
        // F = m a
        for (size_t i = 0; i < N; i++)
        {
                const double M_1 = 1.0 / objs[i].mass;
 
                forces[i].f[0] *= M_1;
                forces[i].f[1] *= M_1;
                forces[i].f[2] *= M_1;
 
                objs[i].vx += forces[i].f[0] * At;
                objs[i].vy += forces[i].f[1] * At;
                objs[i].vz += forces[i].f[2] * At;
 
                objs[i].x += objs[i].vx * At;
                objs[i].y += objs[i].vy * At;
                objs[i].z += objs[i].vz * At;
        }
 
        //      return;
        // Join masses that collided:
        for (int i = N - 1; i >= 0; i--)
        {
                const size_t newObj = lstMass_i_joins_j[i].first;
                if (newObj == string::npos) continue;
 
                const double Mi = objs[i].mass;
                const double Mj = objs[newObj].mass;
                const double newMass = Mi + Mj;
                const double newMass_1 = 1.0 / newMass;
 
                objs[newObj].vx =
                        COLLIS_LOSS * newMass_1 * (Mj * objs[newObj].vx + Mi * objs[i].vx);
                objs[newObj].vy =
                        COLLIS_LOSS * newMass_1 * (Mj * objs[newObj].vy + Mi * objs[i].vy);
                objs[newObj].vz =
                        COLLIS_LOSS * newMass_1 * (Mj * objs[newObj].vz + Mi * objs[i].vz);
 
                objs[newObj].x = newMass_1 * (Mj * objs[newObj].x + Mi * objs[i].x);
                objs[newObj].y = newMass_1 * (Mj * objs[newObj].y + Mi * objs[i].y);
                objs[newObj].z = newMass_1 * (Mj * objs[newObj].z + Mi * objs[i].z);
 
                objs[newObj].mass = newMass;
                objs[newObj].radius = M2R * pow(newMass, 1.0 / 3.0);
                objs[newObj].obj3d->setRadius(objs[newObj].radius);
 
                objs[i].obj3d->setVisibility(false);  // Hide sphere
                objs.erase(objs.begin() + i);
        }
}
 
// ------------------------------------------------------
//                                              MAIN
// ------------------------------------------------------
int main()
{
        try
        {
                GravityDemo();
                return 0;
        }
        catch (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}