CscPhysicsSimulator.h

Go to the documentation of this file.

#ifndef CSCPHYSICSSIMULATOR_H
#define CSCPHYSICSSIMULATOR_H
 
#include "CscCommon.h"
 
#include <functional>
 
#include "CoreObjects/CscMember.h"
#include "CoreObjects/CscMemberJoint.h"
#include "CoreObjects/CscSequence.h"
 
#include <chrono>
#include <cstdlib>
#include <mutex>
#include <thread>
 
#include <iterator>
 
#include <ctime>
#include <iostream>
 
#include "CoreObjects/CscEntityModel.h"
#include "CoreObjects/CscMemberInputPonderation.h"
#include "CoreObjects/CscMemberInputStatus.h"
#include "CoreObjects/CscPropeller.h"
#include "CoreObjects/CscState.h"
#include "CoreObjects/CscWorldElement.h"
#include "CoreObjects/CscWorldEntity.h"
#include "CoreObjects/CscWorldObject.h"
#include "CoreObjects/CscWorldPlace.h"
 
#define FORCE_MEMBER_VALUES_DEFAULT_MAX_ITERATION_NUMBER 1000
#define FORCE_MEMBER_VALUES_DEFAULT_MAX_LOOP_DURATION_MILLIS 50
using std::runtime_error, std::recursive_mutex, std::vector, std::string, std::runtime_error, std::function, std::optional, std::thread, std::isnan;
 
using namespace conscience_utils;
 
 
#pragma region forwardDefs
class btTypedConstraint;
class btHingeConstraint;
class btSliderConstraint;
class btRigidBody;
class btBroadphaseInterface;
class btCollisionDispatcher;
class btDiscreteDynamicsWorld;
class btClock;
class btDefaultCollisionConfiguration;
class btSequentialImpulseConstraintSolver;
class btFixedConstraint;
class btCollisionShape;
class btTransform;
#pragma endregion forwardDefs
 
namespace conscience_core::simulator {
// Collision groups & masks
#define BIT(x) (1 << (x))
enum CollisionTypes {
    COL_NOTHING = 0,            // Collide with nothing
    COL_ROBOT_MEMBERS = BIT(1), // Collide with robot members
    COL_FLOOR = BIT(2),         // Collide with floor
    COL_ENV_OBJ = BIT(3),       // Collide with env objects
    COL_ENV_PLACE = BIT(4)      // Collide with env place
};
 
enum CscMemberPhysicsModelingType { BOX = 0,
                                    WHEEL = 1,
                                    COMPLEX = 2 };
 
typedef function<void(const char *, double *, unsigned long, int, int, bool)> PlayStateListener;
 
 
class CscPhysicsSimulator {
public:
    struct MemberState {
    protected:
        CscPoint3d *const position;
        CscPoint3d *const rotationEuler;
        double rotationQuaternion[4];
        CscPoint3d *const acceleration;
 
        MemberState(CscPoint3d *position, CscPoint3d *euler, CscPoint3d *acceleration);
 
    public:
        const double *const getRotationQuaternionArray() const;
        CscQuaternion getRotationQuaternion() const;
        const CscPoint3d *getPosition() const;
        const CscPoint3d *getRotationEulerAsPoint() const;
        const CscPoint3d *getAcceleration() const;
        virtual ~MemberState();
    };
 
    const unsigned long long id;
 
    typedef int EntityOutputId;
 
    struct BaseEntityOutputPhysicsState {
    public:
        float value;
        float objectiveValue;
        float speedMax;
 
        BaseEntityOutputPhysicsState(float value, float objectiveValue, float speedMax)
            : value(value),
              objectiveValue(objectiveValue),
              speedMax(speedMax) {
        }
 
        virtual ~BaseEntityOutputPhysicsState();
    };
 
    struct PivotPhysicsState : public BaseEntityOutputPhysicsState {
        btHingeConstraint *const bulletConstraint;
        float limitMin;
        float limitMax;
        bool rotationX;
        bool rotationY;
        bool rotationZ;
        float softness;
        float bias;
        float relaxation;
        bool isMotorDisabled;
        float percentSpeedMax;
        float maxImpulse;
        float speedToReachTarget; // In radian per second
 
        PivotPhysicsState(btHingeConstraint *bulletConstraint, const CscMemberJoint *pivot)
            : BaseEntityOutputPhysicsState(pivot->initialValPercent, pivot->initialValPercent, pivot->speedMax),
              bulletConstraint(bulletConstraint),
              limitMin(pivot->initialLimitMin),
              limitMax(pivot->initialLimitMax),
              rotationX(pivot->axeRotationX),
              rotationY(pivot->axeRotationY),
              rotationZ(pivot->axeRotationZ),
              softness(pivot->softness),
              bias(pivot->bias),
              relaxation(pivot->relaxation),
              isMotorDisabled(pivot->free),
              percentSpeedMax(pivot->percentSpeedMax),
              maxImpulse(pivot->maxImpulse),
              speedToReachTarget(pivot->speedMax * pivot->percentSpeedMax / 100.0f) {
        }
        ~PivotPhysicsState() override;
        btHingeConstraint *getBulletConstraint() const {
            return bulletConstraint;
        }
    };
 
    struct WheelPhysicsState : public BaseEntityOutputPhysicsState {
        btHingeConstraint *const bulletConstraint;
        WheelPhysicsState(btHingeConstraint *bulletConstraint, const CscMemberWheel *wheel)
            : BaseEntityOutputPhysicsState(wheel->initialValPercent, wheel->initialValPercent, wheel->speedMax),
              bulletConstraint(bulletConstraint) {
        }
        ~WheelPhysicsState() override;
        btHingeConstraint *getBulletConstraint() const {
            return bulletConstraint;
        }
    };
 
    struct PropellerPhysicsState : public BaseEntityOutputPhysicsState {
        btHingeConstraint *const bulletConstraint;
        PropellerPhysicsState(btHingeConstraint *bulletConstraint, const CscPropeller *propeller)
            : BaseEntityOutputPhysicsState(propeller->initialValPercent, propeller->initialValPercent, propeller->speedMax),
              bulletConstraint(bulletConstraint) {
        }
        ~PropellerPhysicsState() override;
        btHingeConstraint *getBulletConstraint() const {
            return bulletConstraint;
        }
    };
 
    struct TranslationPhysicsState : public BaseEntityOutputPhysicsState {
        btSliderConstraint *const bulletConstraint;
        TranslationPhysicsState(btSliderConstraint *bulletConstraint, const CscTranslation *translation)
            : BaseEntityOutputPhysicsState(translation->initialValPercent, translation->initialValPercent, translation->speedMax),
              bulletConstraint(bulletConstraint) {
        }
        ~TranslationPhysicsState() override;
        btSliderConstraint *getBulletConstraint() const {
            return bulletConstraint;
        }
    };
 
    struct MemberPhysicsState : public MemberState {
        btRigidBody *const body;
 
        unsigned long long nanCountsPosition = 0;
        unsigned long long nanCountsRotation = 0;
        unsigned long long nanCountsAcceleration = 0;
 
        MemberPhysicsState(btRigidBody *const body, const CscMember *member);
        virtual ~MemberPhysicsState();
 
        btCollisionShape *getShape() const;
 
        void updatePosition(double x, double y, double z);
        void updateRotationEuler(double x, double y, double z);
        void updateAcceleration(double x, double y, double z);
        void updateRotationQuaternion(double w, double x, double y, double z);
    };
 
    struct CscEntityPhysicsState {
    public:
        const CscWorldElementId id;
        const CscWorldEntity *originalEntity;
        const CscEntityModel *const model;
        float localGravity = -9.81;
        bool initDone = false;
 
        unsigned long long refreshModelFromPhysicsSimulatorCount = 0;
 
        map<int, MemberPhysicsState *> membersByIndex;
 
        map<EntityOutputId, PivotPhysicsState *> pivots;
        map<EntityOutputId, WheelPhysicsState *> wheels;
        map<EntityOutputId, PropellerPhysicsState *> propellers;
        map<EntityOutputId, TranslationPhysicsState *> translations;
 
        CscEntityPhysicsState(const CscWorldElementId &entityId, const CscWorldEntity *originalEntity, const CscEntityModel *model);
        virtual ~CscEntityPhysicsState();
    };
 
private:
    template <class TOutputState>
    map<EntityOutputId, float> toValues(const map<EntityOutputId, TOutputState> &statesMap) const {
        map<EntityOutputId, float> values;
        for (auto it = statesMap.begin(); it != statesMap.end(); it++) {
            values[it->first] = it->second->value;
        }
        return values;
    }
 
    void initPlaceShapesAndBodies(CscWorldPlace *entityPlace);
    void initObjectShapesAndBodiesSimple(CscWorldObject *object);
    void initObjectShapesAndBodiesHullConvex(CscWorldObject *object);
    void initObjectShapesAndBodiesHullConcave(CscWorldObject *object);
    void initObjectShapesAndBodiesStatic(CscWorldObject *object);
 
    void initDynamicWorld();
    void physicsThreadInit();
 
    void initEntityShapesAndBodies(CscEntityPhysicsState *entityPhysicsState, const CscPoint3dOriented *customPositionAndOrientation = nullptr);
    void initEntityPivot(CscEntityPhysicsState *entityPhysicsState, const CscMemberJoint *currentJoint, int index);
    void initEntityPivots(CscEntityPhysicsState *entityPhysicsState);
    void initEntityTranslation(CscEntityPhysicsState *entityPhysicsState, const CscTranslation *currentTranslation, int index);
    void initEntityTranslations(CscEntityPhysicsState *entityPhysicsState);
    void initEntityWheels(CscEntityPhysicsState *entityPhysicsState);
    void initEntityWheel(CscEntityPhysicsState *entityPhysicsState, const CscMemberWheel *currentWheel, int index);
    void initEntityPropellers(CscEntityPhysicsState *entityPhysicsState);
    void initEntityPropeller(CscEntityPhysicsState *entityPhysicsState, const CscPropeller *currentPropeller, int index);
    void initEntityLense(CscEntityPhysicsState *entityPhysicsState);
 
    /*
     Apply values of parameter state to each entityId’s mechanism (pivots, propellers, wheels and translations)
     */
    void applyOutputs(const CscWorldElementId &entityId, const CscState *outputsValues, bool pivots, bool propellers, bool wheels);
    void applyInputsOnMember(const CscWorldElementId &entityId, const CscMemberInputStatus *inputsIn, bool applyPosition, bool applyRotation, bool applyAcceleration);
    void applyStatePivots(const CscWorldElementId &entityId, const CscState *state);
    void applyStatePropellers(const CscWorldElementId &entityId, const CscState *state);
    void applyStateWheels(const CscWorldElementId &entityId, const CscState *state);
 
    void initObjectIntoPhysicsSimulator(CscWorldObject *object);
    void initObjectShapesAndBodies(CscWorldObject *object);
 
    MemberPhysicsState *getMemberPhysicsState(const CscWorldElementId &entityId, int memberIndex) const {
        bool memberHasState = entities.find(entityId) != entities.end() && entities.at(entityId)->membersByIndex.find(memberIndex) != entities.at(entityId)->membersByIndex.end();
        ASSERT(entityId + " member state not found for index " + to_string(memberIndex), memberHasState);
        if (!memberHasState) {
            throw runtime_error("member state not found for index " + to_string(memberIndex));
        }
        return entities.at(entityId)->membersByIndex.at(memberIndex);
    }
 
    inline btRigidBody *getMemberBody(const CscWorldElementId &entityId, int memberIndex) {
        auto result = getMemberPhysicsState(entityId, memberIndex)->body;
        return result;
    }
 
    inline PivotPhysicsState *getMutablePivotState(const CscWorldElementId &entityId, int id) const {
        bool pivotHasState = entities.find(entityId) != entities.end() && entities.at(entityId)->pivots.find(id) != entities.at(entityId)->pivots.end();
        ASSERT("pivot state not found for id " + to_string(id), pivotHasState);
        if (!pivotHasState) {
            throw runtime_error("pivot state not found for id " + to_string(id));
        }
        return entities.at(entityId)->pivots.at(id);
    }
 
    inline TranslationPhysicsState *getMutableTranslationState(const CscWorldElementId &entityId, int id) const {
        bool translationHasState = entities.find(entityId) != entities.end() && entities.at(entityId)->translations.find(id) != entities.at(entityId)->translations.end();
        ASSERT("translation state not found for id " + to_string(id), translationHasState);
        if (!translationHasState) {
            throw runtime_error("translation state not found for id " + to_string(id));
        }
        return entities.at(entityId)->translations.at(id);
    }
 
    inline WheelPhysicsState *getMutableWheelState(const CscWorldElementId &entityId, int id) const {
        bool wheelHasState = entities.find(entityId) != entities.end() && entities.at(entityId)->wheels.find(id) != entities.at(entityId)->wheels.end();
        ASSERT("wheel state not found for id " + to_string(id), wheelHasState);
        if (!wheelHasState) {
            throw runtime_error("wheel state not found for id " + to_string(id));
        }
        return entities.at(entityId)->wheels.at(id);
    }
 
    inline PropellerPhysicsState *getMutablePropellerState(const CscWorldElementId &entityId, int id) const {
        bool propellerHasState = entities.find(entityId) != entities.end() && entities.at(entityId)->propellers.find(id) != entities.at(entityId)->propellers.end();
        ASSERT("propeller state not found for id " + to_string(id), propellerHasState);
        if (!propellerHasState) {
            throw runtime_error("propeller state not found for id " + to_string(id));
        }
        return entities.at(entityId)->propellers.at(id);
    }
 
    void refreshModelFromPhysicsSimulator();
 
    /*
     * play a state with stepSimulationFutureVision with multi-substeps
     */
    void dividedStepSimulationFutureVision(unsigned umpteenthState, CscSequence *sequence, unsigned nbStateSubstep);
 
    inline CscEntityPhysicsState *getEntityState(const CscWorldElementId &entityId, bool assertFound = false) const {
        auto entityMatch = entities.find(entityId);
        if (entityMatch == entities.end()) {
            if (assertFound) {
                ASSERT("entity not found for id " + entityId, false);
                logger->warn("entity not found for id " + entityId);
            }
            return nullptr;
        }
        return entityMatch->second;
    }
 
    inline const CscEntityModel *getEntityModel(const CscWorldElementId &entityId, bool assertFound = false) const {
        CscEntityPhysicsState *state = getEntityState(entityId, assertFound);
        if (state == nullptr) {
            return nullptr;
        }
        return state->model;
    }
 
protected:
    float floorWidth = 6000.0; // 6000cm == 60 meters
    float floorHeight = 25.4;
    float floorLength = 6000.0; // 6000cm == 60 meters
    float globalGravity = -9.81;
 
    // Physic simulator Bullets attributes
    btBroadphaseInterface *m_broadphase;
    btCollisionDispatcher *m_dispatcher;
    btDefaultCollisionConfiguration *m_collisionConfiguration;
    btSequentialImpulseConstraintSolver *m_solver;
    thread *simulatorPhysicsThread = nullptr;
    bool simulatorPhysicsThreadRunning = false;
    bool simulatorPhysicsThreadStopped = true;
 
    // Default floor
    btRigidBody *floorBody = nullptr;
 
    // World elements: 1) Entities
    map<CscWorldElementId, CscEntityPhysicsState *> entities;
 
    // World elements: 2) Places
    map<CscWorldElementId, btRigidBody *> placeBodies;
 
    // World elements: 3) Other (objects, ...)
    vector<CscWorldObject *> environmentObjects;
    map<CscWorldElementId, btRigidBody *> otherBodies;
    map<CscWorldElementId, btCollisionShape *> otherShapes;
 
    btRigidBody *focusSphereBody = nullptr;
    btRigidBody *focusBarBody = nullptr;
 
    float speedFactor = 1.0;
 
    void setMemberKinematicMode(const CscWorldElementId &entityId, int memberId, bool kinematicMode);
    void memberApplyAcceleration(const CscWorldElementId &entityId, int bodyIndex, float accX, float accY, float accZ);
    void clearAllForces(const CscWorldElementId &entityId);
    void activateEntityPhysics(const CscWorldElementId &entityId);
    bool samePosition(float x1, float y1, float z1, float x2, float y2, float z2, float epsilon);
    bool sameEuler(float yaw1, float pitch1, float roll1, float yaw2, float pitch2, float roll2, float epsilon);
    bool allMembersImmobile(const CscWorldElementId &entityId, float epsilon);
 
    map<int, float> getCurrentPropellers();
    map<int, float> getCurrentWheels();
    map<int, float> getCurrentTranslations();
    map<int, float> getCurrentJoints();
 
    std::unique_ptr<CscLogger> logger;
    recursive_mutex simulatorMutex;
 
    optional<PlayStateListener> onStatePlayedListener = {};
 
    string toDebugLogMemberInputs(const CscWorldElementId &entityId, int memberIndex);
 
    static CscMemberPhysicsModelingType definePhysicsModelingType(const CscMember *member);
    // TODO seems like a duplicate of DaeParser::buildConvexHull => this should be one method in a helper
    static void buildConvexHull(CscWorldElement *object);
    static void buildConcaveHull(CscWorldElement *object, float alphaShapeValue);
 
    void removeAndDeleteBody(btRigidBody *);
    void removeObjectByIndex(int index);
 
    void fireStatePlayed(const CscWorldElementId &entityId, const CscState *state, int duration = 0, int delay = 0, bool relativeValues = false);
 
public:
    const PivotPhysicsState *getPivotState(const CscWorldElementId &entityId, int id) const;
    const PropellerPhysicsState *getPropellerState(const CscWorldElementId &entityId, int id) const;
    const WheelPhysicsState *getWheelState(const CscWorldElementId &entityId, int id) const;
    const TranslationPhysicsState *getTranslationState(const CscWorldElementId &entityId, int id) const;
 
    btDiscreteDynamicsWorld *m_dynamicsWorld = nullptr;
 
    // Constructor & Destructor
    CscPhysicsSimulator(bool integratedPhysicsThread, float simulatorSpeed = 1.0);
    virtual ~CscPhysicsSimulator();
 
    void initFloor();
    void removeFloor();
    void setFloorRotation(float yaw, float pitch, float roll);
    void setFloorFriction(float val);
 
    // Consience AI attributes
    float precisionGyro;
    int precisionAction;
 
    // Conscience Physics attributes
    bool integratedPhysicsThread;
    long curTimeInMicroseconds;
    long prevTimeInMicroseconds;
    float simulatorSpeed; // 1.0 for normal speed, less than 1.0 to decelerate, more than 1.0 to accelerate
 
    btClock *clock = nullptr;
    const float fixedTimeStep = 1.0 / 100.0; // 60 frames per second
    const float minFPS = 16.6666666666f;     // 60 frames per second == 1 frame every 0.0166666666666s == 1 frame every 16.6666666666ms
 
    bool isEntityPhysicsStateReady(const CscWorldElementId &entityId);
 
    void setEntityPhysicsInitDone(const CscWorldElementId &entityId);
    bool isEntityPhysicsStateLoaded(const CscWorldElementId &entityId);
 
    void importEntity(const CscWorldEntity *entity, const CscPoint3dOriented *customPositionAndOrientation = nullptr, bool requiresExternalInit = false);
    void importObjectIntoPhysicsSimulator(CscWorldObject *object);
    void importPlaceIntoPhysicsSimulator(CscWorldPlace *place);
    void importObjectsIntoPhysicsSimulator(vector<CscWorldObject *> *objects);
 
    void updateTranslation(const CscWorldElementId &entityId, int translationId, float val, bool relativeValues = false);
    void updateWheel(const CscWorldElementId &entityId, int wheelId, float val, float cst = 1000.0, float speedCoef = 1.0);
    void updateWheels(const CscWorldElementId &entityId, float val);
    void updatePropeller(const CscWorldElementId &entityId, int propellerId, float val, float cst = 1000.0);
    void updatePivot(const CscWorldElementId &entityId, int pivotId, float newValPercent, bool relativeValues = false);
    void forceMemberValuesAndVerifyState(const CscWorldElementId &entityId, const CscMemberInputStatus *inputsValues, const bool applyPosition[3], const bool applyEuler[3], const bool applyAcceleration[3], optional<int> nbMaxIteration = {}, optional<int> maxLoopDurationMillis = {}, const CscState *verifyState = nullptr);
 
    int moveMemberToStateFromMember(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 tryNumber = 0);
    void moveMember(const CscWorldElementId &entityId, float *vals, int bodyIndex, int isTranslate);
    void memberTeleportToOrigin(const CscWorldElementId &entityId, int bodyIndex, float posX, float posY, float posZ);
    void memberTeleportToRotation(const CscWorldElementId &entityId, int bodyIndex, float yaw, float pitch, float roll);
 
    // https://conscience-robotics.atlassian.net/browse/CS-1470 we should not give access to call listener flag
    void memberTeleportToPositionAndAngle(const CscWorldElementId &entityId, int memberId, const CscPoint3dOriented *target,
                                          bool resetOutputsToInitialVals = false, optional<int> nbMaxIteration = {}, optional<int> maxLoopDurationMillis = {}, bool callPlayStateListener = false);
 
    void entityTeleport(const CscWorldElementId &entityId, const CscPoint3dOriented *target);
 
    // https://conscience-robotics.atlassian.net/browse/CS-1470 we should not give access to call listener flag
    void forceState(const CscWorldElementId &entityId, const CscState *appliedState, int memberIdForInputs, const bool applyPos[3], const bool applyRot[3], const bool applyAcc[3], optional<int> nbMaxIteration = {}, optional<int> maxLoopDurationMillis = {}, bool callPlayStateListener = false);
 
    void playSequence(const CscWorldElementId &entityId, CscState *init, CscSequence *sequence, const map<int, CscPivotMovementType> &pivotMovementByPivotId, bool geneticAi = false, bool extrapolationRealTime = false);
    void updateMemberPhysics(const CscWorldElementId &entityId, int itemId, float mass, float friction, float restitution, float linearDamping, float angularDamping);
    void updatePivotPhysics(const CscWorldElementId &entityId, int id, float min, float max, float softness, float bias, float relaxation, float speedMax, float percentSpeedMax);
    void updatePropellerPhysics(const CscWorldElementId &entityId, int id, float speedMax);
    void updateWheelMaxSpeed(const CscWorldElementId &entityId, int wheelId, float newSpeedMax);
    void updateTranslationPhysics(const CscWorldElementId &entityId, int translationId, float newMaxSpped);
    void updateBehaviorMode(const CscWorldElementId &entityId, int modeId, int memberId);
    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 updatePivotSpeedMax(const CscWorldElementId &entityId, int id, float speedMax);
 
    void disableMembersCollisions(const CscWorldElementId &entityId);
    void enableMembersCollisions(const CscWorldElementId &entityId);
 
    bool hadEntityReachedState(const CscWorldElementId &entityId, const CscState *objectiveState, float epsilon);
 
    float getJointHingeAngle(const CscWorldElementId &entityId, int jointIdIn) const;
    vector<const CscMemberInputStatus *> *getCurrentInputs(const CscWorldElementId &entityId);
 
    virtual CscState *getCurrentState(const CscWorldElementId &entityId);
    CscState *fillStateInputs(const CscWorldElementId &entityId, const CscState *incompleteState);
 
    void stepSimulationRealTime();
    void stepSimulationFutureVision(int millisecondsFromNow, int nbSubSteps = 55);
    void setGravity(float xIn, float yIn, float zIn);
    void setEntityGravity(const CscWorldElementId &entityId, float xIn, float yIn, float zIn);
 
    const CscWorldObject *getObjectByIndex(int index) const {
        return environmentObjects.size() > index ? environmentObjects.at(index) : nullptr;
    }
 
    void setOnStatePlayedListener(PlayStateListener onStatePlayedListener) {
        this->onStatePlayedListener = onStatePlayedListener;
    }
 
    void setSimulatorSpeed(float speed) {
        this->simulatorSpeed = speed;
    }
 
    void setState(const CscWorldElementId &entityId, CscState *state, bool relativeValues = false, int duration = 200);
 
    void playState(const CscWorldElementId &entityId, CscState *state, int duration = 0, int delay = 0, bool relativeValues = false);
 
    void moveObjectToPosition(CscWorldElement *object, const CscPoint3d *targetPosition);
    void moveObjectToRotation(CscWorldElement *object, const CscPoint3d *targetRotation);
    void moveObjectToPositionAndRotation(CscWorldElement *object, const CscPoint3d *targetPosition, const CscPoint3d *targetRotation);
 
    vector<CscPoint3d *> *getEnvObjectShapesVertex(CscWorldElementId objectId);
    int getEnvObjectShapesVertexCount(CscWorldElementId objectId);
 
    void removeEntity(const CscWorldElementId &entityId);
 
    void clearPlaces();
    void clearObjects();
 
    void removeConscienceMemberFrontFocus();
    CscPoint3dOriented *getFocusBarPoint();
 
    btFixedConstraint *visionCnt1 = NULL;
    btHingeConstraint *visionCnt2 = NULL;
    CscPoint3dOriented *getFocusSpherePoint();
    void focusVisionIntoPosition(float posX, float posY, float posZ);
 
    void setAllPivotsToRelaxedMode(const CscWorldElementId &entityId);
    void setAllPivotsMotorDisabled(const CscWorldElementId &entityId, bool enabled);
 
    void unitUpdatePivots();
    void unitUpdateTranslations();
 
    void removeObjectById(CscWorldElementId objectId);
    void removeObjectByName(string name);
    void removePlaceById(string placeId);
 
    vector<CscPoint3d *> performRayScale(const CscPoint3d *position, const vector<const CscPoint3d *> &rayEnds, optional<int> collisionFilterGroup = {COL_ENV_PLACE | COL_FLOOR | COL_ROBOT_MEMBERS | COL_ENV_OBJ}, optional<int> collisionFilterMask = {COL_ENV_PLACE | COL_FLOOR | COL_ENV_OBJ});
 
    float getPivotLimitMin(const CscWorldElementId &entityId, int pivotId) const;
    float getPivotLimitMax(const CscWorldElementId &entityId, int pivotId) const;
    const MemberState *getMemberStateById(const CscWorldElementId &entityId, int id) const;
    const MemberState *getMemberState(const CscWorldElementId &entityId, const CscMember *member) const;
    const MemberState *getMemberStateByName(const CscWorldElementId &entityId, const string &memberName) const;
 
    string getPivotOtherMember(const CscWorldElementId &entityId, const string &memberName);
 
    vector<CscState *> dividingOneStateIntoSubStates(CscState *stateToDivide, CscState *currentState, int durationStateToDivide, int nbStateToDivide, float durationLastSubStep, const map<int, CscPivotMovementType> &pivotMovementByPivotId);
 
    CscSequence *dividingAllStatesOfOneSequenceIntoSubStates(CscSequence *sequenceToDivide, CscState *initialState, const map<int, CscPivotMovementType> &pivotMovementByPivotId);
 
    map<int, CscPoint3d *> getCenterOfMassOfAllMemberByMemberId(const CscWorldElementId &entityId);
 
    CscPoint3d *findCenterOfMass(const CscEntityModel *entityModel, map<int, CscPoint3d *> centerOfMassOfAllMemberByMemberId);
};
 
}
 
#endif