test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.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() : obj3d() {}
    double x{0}, y{0}, z{0};
    double vx{0}, vy{0}, vz{0};
    double mass{1};
    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 =
            opengl::CGridPlaneXY::Create(-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 = opengl::CSphere::Create();

        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 (const std::exception& e)
    {
        std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
    catch (...)
    {
        printf("Untyped exception!!");
        return -1;
    }
}