CscEnvironmentSimulator.h

Go to the documentation of this file.

//
//  CscEnvironmentSimulator.h
//  ConscienceRobotManager
//
//  Created by Conscience_Hazar on 05/03/2019.
//  Copyright © 2019 IliesZaoui. All rights reserved.
//
 
#ifndef CscEnvironmentSimulator_h
#define CscEnvironmentSimulator_h
 
#include "CscCommon.h"
#include <cstring>
#include <iostream>
#include <iterator>
#include <mutex>
 
#include "CoreObjects/CscEntityModel.h"
#include "CoreObjects/CscWorldEntity.h"
#include "CoreObjects/CscWorldImage.h"
#include "CoreObjects/CscWorldLine.h"
#include "CoreObjects/CscWorldObject.h"
#include "CoreObjects/CscWorldPlace.h"
#include "Simulator/CscPhysicsSimulator.h"
 
#include "Axiomes/CscGrid.h"
 
 
using std::mutex, std::function;
 
using namespace conscience_core::core_objects;
using namespace conscience_core::simulator;
using MemberState = CscPhysicsSimulator::MemberState;
 
class CscEnvironmentSimulator {
private:
    CscPhysicsSimulator *physicSimulator = nullptr;
    map<CscWorldElementId, const CscWorldEntity *> *entities = nullptr;
    map<CscWorldElementId, function<void()>> onRemoveEntityCallbacks;
    vector<CscWorldObject *> *objects = nullptr;
    vector<CscWorldPlace *> *places = nullptr;
    vector<CscWorldElement *> *otherElements = nullptr;
    map<string, CscWorldImage *> *sceneImages = nullptr;
 
    CscGrid *sceneFloor = nullptr;
    CscPoint3d *gnssReferencePosition = nullptr;
 
    mutex entitiesMutex;
    mutex placesMutex;
    mutable mutex objectsMutex;
    mutex otherElementsMutex;
    mutex imagesMutex;
    mutex gnssPositionOperationMutex;
 
    inline static std::atomic<unsigned long long> lastId = 0;
    unsigned long long id;
 
    unique_ptr<CscLogger> logger;
 
    int getObjectIndexByName(const string &name) const;
    int getOtherElementIndexByName(const string &name) const;
 
    void doRemoveEntity(const CscWorldElementId &entityId);
 
public:
    CscEnvironmentSimulator(bool autoIncrement);
    virtual ~CscEnvironmentSimulator();
 
    void clear();
 
    virtual vector<CscWorldObject *> getObjects(const function<bool(const CscWorldObject *)> filter = [](const CscWorldObject *) { return true; }) const;
 
    CscWorldElement *getElementByName(const string &nameIn);
 
    float *getConscienceSimulationWorldStatus(bool rotationsAsEuler = false);
 
    vector<CscPoint3d *> *getVertexFromStrategy(CscWorldElementId entityId, int strategy);
    vector<CscPoint3d *> *getPatchFromStrategy(CscWorldElementId entityId, int strategy);
 
    void importObject(CscWorldObject *object);
    void importObjects(vector<CscWorldObject *> *objects);
    void importPlace(CscWorldPlace *place, bool removeOthers = true);
    void importLine(CscWorldLine *line);
    void importImage(CscWorldImage *sceneImage);
    void removeOtherElementByName(const string &name);
 
    void importPointsAsObjects(vector<CscPoint3d *> *points);
 
    void importEntity(const CscWorldEntity *entity, bool requiresExternalInit = false, const optional<function<void()>> &onRemoved = {});
    bool removeEntity(const CscWorldElementId &entityId);
    virtual void clearEntities();
    bool hasEntity(const CscWorldElementId &entityId);
 
    CscPhysicsSimulator *getPhysicSimulator() const;
    void setFocusedObjectByIndex(int objectIndex);
    CscWorldObject *getFocusedObjectCurrent() const;
    int countObjects();
    virtual void clearObjects();
    int currentFocusedObjectIndex;
    CscWorldObject *getObjectById(const CscWorldElementId objectId) const;
    const CscPoint3d *getObjectPositionInSimulator(const string &serialNumber) const;
 
    CscWorldObject *getObjectByModelId(int objectModelId);
    virtual vector<CscWorldObject *> getObjectsByModelId(int objectModelId);
 
    CscWorldPlace *getPlace();
    CscWorldPlace *getPlaceById(const CscWorldElementId &placeId);
 
    const vector<CscWorldPlace *> getPlaces(function<bool(const CscWorldPlace *)> filter = [](const CscWorldPlace *) { return true; });
 
    const CscWorldEntity *getEntity(const CscWorldElementId &entityId) const;
    virtual vector<CscWorldEntity *> getEntities(const function<bool(const CscWorldEntity *)> filter = [](const CscWorldEntity *) { return true; });
    virtual vector<string> getEntitySerialNumbers(const function<bool(const CscWorldEntity *)> filter = [](const CscWorldEntity *) { return true; });
    const CscEntityModel *getEntityModel(const CscWorldElementId &entityId) const;
 
    const MemberState *getEntityMemberStateById(const CscWorldElementId &entityId, int id) const;
    const MemberState *getEntityMemberState(const CscWorldElementId &entityId, const CscMember *member) const;
    const MemberState *getEntityMemberStateByName(const CscWorldElementId &entityId, const string &memberName) const;
 
    const CscPoint3d *getEntityPosition(const CscWorldElementId &entityId) const;
    CscPoint3dOriented getEntityPositionAndRotation(const CscWorldElementId &entityId) const;
    CscPoint3d getEntityAcceleration(const CscWorldElementId &entityId) const;
    pair<CscPoint3d, CscQuaternion> getEntityPositionAndRotationQuaternion(const CscWorldElementId &entityId) const;
 
    void doWithEntities(function<void(const map<CscWorldElementId, const CscWorldEntity *> &entities)> action);
 
    void doWithObjects(function<void(const vector<CscWorldObject *> &objects)> action);
 
    CscState *getEntityState(const CscWorldElementId &entityId) const;
 
    void forceState(const CscWorldElementId &entityId, CscState *aState, int memberIdForInputs, const bool applyPos[3], const bool applyRot[3], const bool applyAcc[3], optional<int> nbMaxIteration = {}, optional<int> maxLoopDurationMillis = {});
    void playSequence(const CscWorldElementId &entityId, CscState *init, CscSequence *sequence, const map<int, CscPivotMovementType> &pivotMovementByPivotId, bool geneticAi = false, bool extrapolationRealTime = false);
    void playState(const CscWorldElementId &entityId, CscState *state, int duration, int delay, bool relativeValues);
 
    void setAllPivotsToRelaxedMode(const CscWorldElementId &entityId);
    void forceMemberValues(const CscWorldElementId &entityId, const CscMemberInputStatus *inputsValues, const bool applyPosition[3], const bool applyEuler[3], const bool applyAcceleration[3], optional<int> nbMaxIteration = {}, optional<int> maxLoopDurationMillis = {});
    int moveMemberToStateFromMember(const CscWorldElementId &entityId, const CscMemberInputStatus *inputsValues, const bool applyPosition[3], const bool applyEuler[3], const bool applyAcceleration[3], int nbMaxIteration = 40, int tryNumber = 0);
    float getJointHingeAngle(const CscWorldElementId &entityId, int jointIdIn) const;
 
    void updateMemberEulers(const CscWorldElementId &entityId, int memberId, double eulerX, double eulerY, double eulerZ);
    void updateMemberPosition(const CscWorldElementId &entityId, int memberId, float posX, float posY, float posZ);
 
    void moveObjectToPosition(CscWorldElement *object, const CscPoint3d *targetPosition);
    void moveObjectToRotation(CscWorldElement *object, const CscPoint3d *targetRotation);
    void moveObjectToPositionAndRotation(CscWorldElement *object, const CscPoint3d *targetPosition, const CscPoint3d *targetRotation);
 
    virtual void clearPlaces();
    void loadDefaultPlaceFloor();
    void clearOtherElements();
    CscGrid *getSceneFloor() const;
 
    void focusVisionIntoPosition(float posX, float posY, float posZ);
 
    CscWorldObject *getObjectByName(const string &name);
    CscWorldObject *getObjectByPosition(CscPoint3d *position, float radius);
    virtual void removeObjectById(CscWorldElementId objectId);
    void removeObjectByName(const string &name);
    void removeObjectsByModelId(int objectModelId);
    void removeObjectsByTag(const string &searchedTag);
    bool removePlaceById(const string &placeId);
 
    bool hasObjectWithModelId(int objectModelId);
 
    void dump(fs::path outPath = fs::path(string(CSC_DEFAULT_OUT_DIR) + "/envSim_" + to_string(nowMillis()) + ".png")) const;
 
    CscPoint3d *computeConsciencePositionFromGnssPosition(const CscPoint3d *gnssPosition, const string &projection);
    CscPoint3d *computeGnssPositionFromConsciencePosition(const CscPoint3d *consciencePosition, const string &projection);
 
    vector<CscPoint3d *> *computeGnssPositionsFromConsciencePositions(vector<CscPoint3d *> *consciencePositions, const string &projection);
 
    void setGnssReferencePosition(CscPoint3d *gnssPoint);
 
    bool gnssReferencePositionIsSet();
 
    void computeGnssReferencePositionWithConsciencePosition(CscPoint3d *gnssPoint, CscPoint3d *consciencePosition, const string &projection);
};
 
#endif /* CscEnvironmentSimulator_h */