World.cpp

Go to the documentation of this file.

#include <tdme/engine/physics/World.h>
 
#include <algorithm>
#include <iostream>
#include <map>
#include <memory>
#include <string>
#include <unordered_set>
 
#include <reactphysics3d/utils/DefaultLogger.h>
 
#include <reactphysics3d/collision/shapes/AABB.h>
#include <reactphysics3d/collision/ContactManifold.h>
#include <reactphysics3d/collision/OverlapCallback.h>
#include <reactphysics3d/collision/RaycastInfo.h>
#include <reactphysics3d/constraint/ContactPoint.h>
#include <reactphysics3d/constraint/FixedJoint.h>
#include <reactphysics3d/constraint/Joint.h>
#include <reactphysics3d/engine/PhysicsWorld.h>
#include <reactphysics3d/engine/EventListener.h>
#include <reactphysics3d/mathematics/Ray.h>
#include <reactphysics3d/mathematics/Vector3.h>
 
#include <tdme/tdme.h>
#include <tdme/engine/physics/Body.h>
#include <tdme/engine/physics/BodyHierarchy.h>
#include <tdme/engine/physics/CollisionResponse.h>
#include <tdme/engine/physics/CollisionResponse_Entity.h>
#include <tdme/engine/physics/WorldListener.h>
#include <tdme/engine/primitives/BoundingBox.h>
#include <tdme/engine/primitives/BoundingVolume.h>
#include <tdme/engine/primitives/LineSegment.h>
#include <tdme/engine/primitives/OrientedBoundingBox.h>
#include <tdme/engine/Engine.h>
#include <tdme/engine/Entity.h>
#include <tdme/engine/Rotation.h>
#include <tdme/engine/Transform.h>
#include <tdme/math/Math.h>
#include <tdme/math/Matrix4x4.h>
#include <tdme/math/Quaternion.h>
#include <tdme/math/Vector3.h>
#include <tdme/utilities/Console.h>
#include <tdme/utilities/StringTools.h>
#include <tdme/utilities/Time.h>
 
using std::find;
using std::make_unique;
using std::map;
using std::remove;
using std::string;
using std::to_string;
using std::unordered_set;
 
using tdme::engine::physics::Body;
using tdme::engine::physics::BodyHierarchy;
using tdme::engine::physics::CollisionResponse;
using tdme::engine::physics::CollisionResponse_Entity;
using tdme::engine::physics::World;
using tdme::engine::physics::WorldListener;
using tdme::engine::primitives::BoundingBox;
using tdme::engine::primitives::BoundingVolume;
using tdme::engine::primitives::LineSegment;
using tdme::engine::primitives::OrientedBoundingBox;
using tdme::engine::Engine;
using tdme::engine::Entity;
using tdme::engine::Rotation;
using tdme::engine::Transform;
using tdme::math::Math;
using tdme::math::Matrix4x4;
using tdme::math::Quaternion;
using tdme::math::Vector3;
using tdme::utilities::Console;
using tdme::utilities::StringTools;
using tdme::utilities::Time;
 
World::World(const string& id)
{
    //
    reactphysics3d::PhysicsWorld::WorldSettings worldSettings;
    worldSettings.worldName = id;
 
    //
    world = physicsCommon.createPhysicsWorld(worldSettings);
}
 
World::~World()
{
    //
    worldListeners.clear();
    reset();
    physicsCommon.destroyPhysicsWorld(world);
}
 
void World::reset()
{
    // joints
    {
        vector<string> jointIds;
        for (const auto& [jointId, joint]: jointsById) jointIds.push_back(jointId);
        for (const auto& jointId: jointIds) removeJoint(jointId);
    }
    // bodies
    {
        auto _bodies = bodies;
        for (auto body: _bodies) removeBody(body->getId());
        bodyCollisionsLastFrame.clear();
    }
}
 
Body* World::addRigidBody(const string& id, uint16_t collisionTypeId, bool enabled, const Transform& transform, float restitution, float friction, float mass, const Vector3& inertiaTensor, const vector<BoundingVolume*>& boundingVolumes, bool hierarchy)
{
    removeBody(id);
    //
    auto body =
        hierarchy == true?
            new BodyHierarchy(this, id, Body::BODYTYPE_DYNAMIC, collisionTypeId, enabled, transform, restitution, friction, mass, inertiaTensor):
            new Body(this, id, Body::BODYTYPE_DYNAMIC, collisionTypeId, enabled, transform, restitution, friction, mass, inertiaTensor, boundingVolumes);
    //
    if (hierarchy == true) {
        static_cast<BodyHierarchy*>(body)->addBody(id, Transform(), boundingVolumes);
        static_cast<BodyHierarchy*>(body)->update();
    }
    //
    bodies.push_back(body);
    rigidBodiesDynamic.push_back(body);
    bodiesById[id] = body;
    //
    for (auto listener: worldListeners) {
        listener->onAddedBody(id, Body::BODYTYPE_DYNAMIC, collisionTypeId, enabled, transform, restitution, friction, mass, inertiaTensor, boundingVolumes);
    }
    //
    return body;
}
 
Body* World::addStaticCollisionBody(const string& id, uint16_t collisionTypeId, bool enabled, const Transform& transform, const vector<BoundingVolume*>& boundingVolumes, bool hierarchy) {
    removeBody(id);
    //
    auto body =
        hierarchy == true?
            new BodyHierarchy(this, id, Body::BODYTYPE_COLLISION_STATIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor()):
            new Body(this, id, Body::BODYTYPE_COLLISION_STATIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    //
    if (hierarchy == true) {
        static_cast<BodyHierarchy*>(body)->addBody(id, Transform(), boundingVolumes);
        static_cast<BodyHierarchy*>(body)->update();
    }
    //
    bodies.push_back(body);
    bodiesById[id] = body;
    //
    for (auto listener: worldListeners) {
        listener->onAddedBody(id, Body::BODYTYPE_COLLISION_STATIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    }
    //
    return body;
}
 
Body* World::addDynamicCollisionBody(const string& id, uint16_t collisionTypeId, bool enabled, const Transform& transform, const vector<BoundingVolume*>& boundingVolumes, bool hierarchy) {
    removeBody(id);
    //
    auto body =
        hierarchy == true?
            new BodyHierarchy(this, id, Body::BODYTYPE_COLLISION_DYNAMIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor()):
            new Body(this, id, Body::BODYTYPE_COLLISION_DYNAMIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    //
    if (hierarchy == true) {
        static_cast<BodyHierarchy*>(body)->addBody(id, Transform(), boundingVolumes);
        static_cast<BodyHierarchy*>(body)->update();
    }
    //
    bodies.push_back(body);
    bodiesById[id] = body;
    //
    for (auto listener: worldListeners) {
        listener->onAddedBody(id, Body::BODYTYPE_COLLISION_DYNAMIC, collisionTypeId, enabled, transform, 0.0f, 0.0f, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    }
    //
    return body;
}
 
Body* World::addStaticRigidBody(const string& id, uint16_t collisionTypeId, bool enabled, const Transform& transform, float friction, const vector<BoundingVolume*>& boundingVolumes, bool hierarchy)
{
    removeBody(id);
    //
    auto body =
        hierarchy == true?
            new BodyHierarchy(this, id, Body::BODYTYPE_STATIC, collisionTypeId, enabled, transform, 0.0f, friction, 0.0f, Body::getNoRotationInertiaTensor()):
            new Body(this, id, Body::BODYTYPE_STATIC, collisionTypeId, enabled, transform, 0.0f, friction, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    //
    if (hierarchy == true) {
        static_cast<BodyHierarchy*>(body)->addBody(id, Transform(), boundingVolumes);
        static_cast<BodyHierarchy*>(body)->update();
    }
    //
    bodies.push_back(body);
    bodiesById[id] = body;
    //
    for (auto listener: worldListeners) {
        listener->onAddedBody(id, Body::BODYTYPE_STATIC, collisionTypeId, enabled, transform, 0.0f, friction, 0.0f, Body::getNoRotationInertiaTensor(), boundingVolumes);
    }
    //
    return body;
}
 
BodyHierarchy* World::getBodyHierarchy(const string& id) {
    auto body = getBody(id);
    if (body == nullptr) return nullptr;
    return dynamic_cast<BodyHierarchy*>(body);
}
 
void World::removeBody(const string& id) {
    auto bodyByIdIt = bodiesById.find(id);
    if (bodyByIdIt != bodiesById.end()) {
        auto body = bodyByIdIt->second;
        bodies.erase(remove(bodies.begin(), bodies.end(), body), bodies.end());
        rigidBodiesDynamic.erase(remove(rigidBodiesDynamic.begin(), rigidBodiesDynamic.end(), body), rigidBodiesDynamic.end());
        bodiesById.erase(bodyByIdIt);
        for (auto listener: worldListeners) {
            listener->onRemovedBody(id, body->getType(), body->getCollisionTypeId());
        }
        //
        delete body;
    }
}
 
void World::addFixedJoint(const string& id, Body* body1, Body* body2) {
    removeJoint(id);
    if (body1->rigidBody == nullptr) {
        Console::println("World::createFixedJoint(): body1: no rigid body attached");
        return;
    } else
    if (body2->rigidBody == nullptr) {
        Console::println("World::createFixedJoint(): body2: no rigid body attached");
        return;
    }
    Vector3 anchorPoint = body1->getTransform().getTranslation().clone().add(body2->getTransform().getTranslation()).scale(0.5f);
    reactphysics3d::FixedJointInfo jointInfo(body1->rigidBody, body2->rigidBody, reactphysics3d::Vector3(anchorPoint.getX(), anchorPoint.getY(), anchorPoint.getZ()));
    jointInfo.isCollisionEnabled = false;
    jointsById[id] = dynamic_cast<reactphysics3d::FixedJoint*>(world->createJoint(jointInfo));
}
 
void World::removeJoint(const string& id) {
    auto jointByIdIt = jointsById.find(id);
    if (jointByIdIt != jointsById.end()) {
        auto joint = jointByIdIt->second;
        world->destroyJoint(joint);
        jointsById.erase(jointByIdIt);
    }
}
 
void World::update(float deltaTime)
{
    if (deltaTime < Math::EPSILON) return;
 
    // do the job
    world->update(deltaTime);
 
    /*
    // TODO: collision events
    // collision events
    {
        // fire on collision begin, on collision
        map<string, BodyCollisionStruct> bodyCollisionsCurrentFrame;
        CollisionResponse collision;
        auto manifolds = world->getContactsList();
        for (auto manifold: manifolds) {
            auto body1 = static_cast<Body*>(manifold->getBody1()->getUserData());
            auto body2 = static_cast<Body*>(manifold->getBody2()->getUserData());
            BodyCollisionStruct bodyCollisionStruct;
            bodyCollisionStruct.body1Id = body1->getId();
            bodyCollisionStruct.body2Id = body2->getId();
            string bodyKey = bodyCollisionStruct.body1Id + "," + bodyCollisionStruct.body2Id;
            string bodyKeyInverted = bodyCollisionStruct.body2Id + "," + bodyCollisionStruct.body1Id;
            bodyCollisionsCurrentFrame[bodyKey] = bodyCollisionStruct;
            for (int i=0; i<manifold->getNbContactPoints(); i++) {
                auto contactPoint = manifold->getContactPoints();
                while (contactPoint != nullptr) {
                    // construct collision
                    auto entity = collision.addResponse(-contactPoint->getPenetrationDepth());
                    auto normal = contactPoint->getNormal();
                    entity->setNormal(Vector3(normal.x, normal.y, normal.z));
                    auto shape1 = manifold->getShape1();
                    auto shape2 = manifold->getShape2();
                    auto shapeLocalToWorldTransform1 = shape1->getLocalToWorldTransform();
                    auto shapeLocalToWorldTransform2 = shape2->getLocalToWorldTransform();
                    auto& localPoint1 = contactPoint->getLocalPointOnShape1();
                    auto& localPoint2 = contactPoint->getLocalPointOnShape2();
                    auto worldPoint1 = shapeLocalToWorldTransform1 * localPoint1;
                    auto worldPoint2 = shapeLocalToWorldTransform2 * localPoint2;
                    entity->addHitPoint(Vector3(worldPoint1.x, worldPoint1.y, worldPoint1.z));
                    entity->addHitPoint(Vector3(worldPoint2.x, worldPoint2.y, worldPoint2.z));
                    contactPoint = contactPoint->getNext();
                    // fire events
                    if (bodyCollisionsLastFrame.find(bodyKey) == bodyCollisionsLastFrame.end() &&
                        bodyCollisionsLastFrame.find(bodyKeyInverted) == bodyCollisionsLastFrame.end()) {
                        // fire on collision begin
                        body1->fireOnCollisionBegin(body2, collision);
                    }
                    // fire on collision
                    body1->fireOnCollision(body2, collision);
                    // reset collision
                    collision.reset();
                }
            }
        }
 
        // fire on collision end
        //  check each collision last frame that disappeared in current frame
        for (auto it: bodyCollisionsLastFrame) {
            BodyCollisionStruct* bodyCollisionStruct = &it.second;
            {
                string bodyKey = bodyCollisionStruct->body1Id + "," + bodyCollisionStruct->body2Id;
                auto bodyCollisionsCurrentFrameIt = bodyCollisionsCurrentFrame.find(bodyKey);
                if (bodyCollisionsCurrentFrameIt != bodyCollisionsCurrentFrame.end()) continue;
            }
            {
                string bodyKey = bodyCollisionStruct->body2Id + "," + bodyCollisionStruct->body1Id;
                auto bodyCollisionsCurrentFrameIt = bodyCollisionsCurrentFrame.find(bodyKey);
                if (bodyCollisionsCurrentFrameIt != bodyCollisionsCurrentFrame.end()) continue;
            }
            auto body1It = bodiesById.find(bodyCollisionStruct->body1Id);
            auto body1 = body1It == bodiesById.end()?nullptr:body1It->second;
            auto body2It = bodiesById.find(bodyCollisionStruct->body2Id);
            auto body2 = body2It == bodiesById.end()?nullptr:body2It->second;
            if (body1 == nullptr || body2 == nullptr) continue;
            body1->fireOnCollisionEnd(body2);
        }
        // swap rigid body collisions current and last frame
        bodyCollisionsLastFrame = bodyCollisionsCurrentFrame;
    }
    */
 
    // update transform for rigid body
    for (auto body: rigidBodiesDynamic) {
        // skip if disabled
        if (body->isEnabled() == false) {
            continue;
        }
        // skip if static or sleeping
        if (body->isSleeping() == true) {
            continue;
        }
 
        // rp3d transform
        const auto& transform = body->rigidBody->getTransform();
        const auto& transformPosition = transform.getPosition();
        const auto& transformOrientation = transform.getOrientation();
 
        // tdme2 transform
        auto& physicsTransform = body->transform;
 
        // rotations
        // keep care that only 1 rotation exists
        while (physicsTransform.getRotationCount() > 1) {
            physicsTransform.removeRotation(physicsTransform.getRotationCount() - 1);
        }
        if (physicsTransform.getRotationCount() < 1) {
            physicsTransform.addRotation(Vector3(0.0f, 1.0f, 0.0f), 0.0f);
        }
 
        // rotations
        physicsTransform.getRotation(0).fromRotation(
            Rotation::fromQuaternion(
                Quaternion(
                    transformOrientation.x,
                    transformOrientation.y,
                    transformOrientation.z,
                    transformOrientation.w
                )
            )
        );
        // scale
        physicsTransform.setScale(body->transformScale);
        // translation
        physicsTransform.setTranslation(Vector3(transformPosition.x, transformPosition.y, transformPosition.z));
        // done
        physicsTransform.update();
    }
}
 
void World::synchronize(Engine* engine)
{
    for (auto i = 0; i < rigidBodiesDynamic.size(); i++) {
        // update rigid body
        auto body = rigidBodiesDynamic[i];
 
        // skip on sleeping objects
        if (body->isSleeping() == true) continue;
 
        // synch with engine entity
        auto entity = engine->getEntity(body->getId());
        if (entity == nullptr) {
            Console::println("World::entity '" + body->getId() + "' not found");
            continue;
        }
 
        // enable
        entity->setEnabled(body->isEnabled());
 
        // apply transform to engine entity
        if (body->isEnabled() == true) entity->setTransform(body->transform);
    }
}
 
Body* World::determineHeight(uint16_t collisionTypeIds, float stepUpMax, const Vector3& point, Vector3& heightPoint, float minHeight, float maxHeight)
{
    // TODO: we seem to have a bug here
    //
    class CustomCallbackClass : public reactphysics3d::RaycastCallback {
    public:
        CustomCallbackClass(float stepUpMax, const Vector3& point, float height = 10000.0f): stepUpMax(stepUpMax), point(point), height(height), body(nullptr) {
        }
        virtual reactphysics3d::decimal notifyRaycastHit(const reactphysics3d::RaycastInfo& info) {
            Vector3 hitPoint(info.worldPoint.x, info.worldPoint.y, info.worldPoint.z);
            auto _body = static_cast<Body*>(info.body->getUserData());
            if (hitPoint.getY() < height && hitPoint.getY() <= point.getY() + stepUpMax) {
                height = hitPoint.getY();
                body = static_cast<Body*>(info.body->getUserData());
            }
            return reactphysics3d::decimal(info.hitFraction);
        };
        Body* getBody() {
            return body;
        }
        float getHeight() {
            return height;
        }
    private:
        float stepUpMax;
        Vector3 point;
        float height;
        Body* body;
    };
    reactphysics3d::Vector3 startPoint(point.getX(), maxHeight, point.getZ());
    reactphysics3d::Vector3 endPoint(point.getX(), minHeight, point.getZ());
    reactphysics3d::Ray ray(startPoint, endPoint);
    CustomCallbackClass customCallbackObject(stepUpMax, point, maxHeight);
    world->raycast(ray, &customCallbackObject, collisionTypeIds);
    if (customCallbackObject.getBody() != nullptr) {
        heightPoint.set(point);
        heightPoint.setY(customCallbackObject.getHeight());
        //
        return customCallbackObject.getBody();
    } else {
        return nullptr;
    }
}
 
Body* World::doRayCasting(uint16_t collisionTypeIds, const Vector3& start, const Vector3& end, Vector3& hitPoint, const string& actorId)
{
    class CustomCallbackClass : public reactphysics3d::RaycastCallback {
    public:
        CustomCallbackClass(const string& actorId): actorId(actorId), body(nullptr) {
        }
        virtual reactphysics3d::decimal notifyRaycastHit(const reactphysics3d::RaycastInfo& info) {
            auto _body = static_cast<Body*>(info.body->getUserData());
            if (actorId.size() == 0 || _body->getId() != actorId) {
                body = _body;
                hitPoint.set(info.worldPoint.x, info.worldPoint.y, info.worldPoint.z);
                return reactphysics3d::decimal(info.hitFraction);
            } else {
                return reactphysics3d::decimal(1.0);
            }
        };
        Body* getBody() {
            return body;
        }
        const Vector3& getHitPoint() {
            return hitPoint;
        }
    private:
        string actorId;
        Vector3 hitPoint;
        Body* body;
    };
    reactphysics3d::Vector3 startPoint(start.getX(), start.getY(), start.getZ());
    reactphysics3d::Vector3 endPoint(end.getX(), end.getY(), end.getZ());
    reactphysics3d::Ray ray(startPoint, endPoint);
    CustomCallbackClass customCallbackObject(actorId);
    world->raycast(ray, &customCallbackObject, collisionTypeIds);
    if (customCallbackObject.getBody() != nullptr) {
        hitPoint.set(customCallbackObject.getHitPoint());
        return customCallbackObject.getBody();
    } else {
        return nullptr;
    }
}
 
bool World::doesCollideWith(uint16_t collisionTypeIds, Body* body, vector<Body*>& collisionBodies) {
    // callback
    class CustomOverlapCallback: public reactphysics3d::OverlapCallback {
        public:
            CustomOverlapCallback(int collisionTypeIds, Body* body, vector<Body*>& collisionBodies): collisionTypeIds(collisionTypeIds), body(body), collisionBodies(collisionBodies) {
            }
 
            void onOverlap(CallbackData &callbackData) {
                for (auto i = 0; i < callbackData.getNbOverlappingPairs(); i++) {
                    auto overlappingPair = callbackData.getOverlappingPair(i);
                    auto body1 = static_cast<Body*>(overlappingPair.getBody1()->getUserData());
                    auto body2 = static_cast<Body*>(overlappingPair.getBody2()->getUserData());
                    if (body != body1 && ((body1->getCollisionTypeId() & collisionTypeIds) != 0)) collisionBodies.push_back(body1);
                    if (body != body2 && ((body2->getCollisionTypeId() & collisionTypeIds) != 0)) collisionBodies.push_back(body2);
                }
            }
        private:
            int collisionTypeIds;
            Body* body;
            vector<Body*>& collisionBodies;
    };
 
    // do the test
    CustomOverlapCallback customOverlapCallback(collisionTypeIds, body, collisionBodies);
    world->testOverlap(body->rigidBody, customOverlapCallback);
 
    // done
    return collisionBodies.size() > 0;
}
 
bool World::doesCollideWith(uint16_t collisionTypeIds, const Transform& transform, vector<BoundingVolume*> boundingVolumes, vector<Body*>& collisionBodies) {
    auto collisionBody = addStaticCollisionBody("tdme.world->doescollidewith", 32768, true, transform, boundingVolumes);
    doesCollideWith(collisionTypeIds, collisionBody, collisionBodies);
    removeBody("tdme.world->doescollidewith");
    return collisionBodies.size() > 0;
}
 
bool World::doCollide(Body* body1, Body* body2) {
    return world->testOverlap(body1->rigidBody, body2->rigidBody);
}
 
bool World::getCollisionResponse(Body* body1, Body* body2, CollisionResponse& collision) {
    // callback
    class CustomCollisionCallback: public reactphysics3d::CollisionCallback {
        public:
            CustomCollisionCallback(CollisionResponse& collision): collision(collision) {
            }
 
            void onContact(const CallbackData &callbackData) {
                for (auto i = 0; i < callbackData.getNbContactPairs(); i++) {
                    auto contactPair = callbackData.getContactPair(i);
                    auto body1 = contactPair.getBody1();
                    auto body2 = contactPair.getBody2();
                    auto collider1 = contactPair.getCollider1();
                    auto collider2 = contactPair.getCollider2();
                    for (auto j = 0; j < contactPair.getNbContactPoints(); j++) {
                        auto contactPoint = contactPair.getContactPoint(j);
                        // construct collision
                        auto entity = collision.addResponse(-contactPoint.getPenetrationDepth());
                        auto normal = contactPoint.getWorldNormal();
                        entity->setNormal(Vector3(normal.x, normal.y, normal.z));
                        auto collider1LocalToWorldTransform1 = collider1->getLocalToWorldTransform();
                        auto collider2LocalToWorldTransform2 = collider2->getLocalToWorldTransform();
                        const auto& localPoint1 = contactPoint.getLocalPointOnCollider1();
                        const auto& localPoint2 = contactPoint.getLocalPointOnCollider2();
                        auto worldPoint1 = collider1LocalToWorldTransform1 * localPoint1;
                        auto worldPoint2 = collider2LocalToWorldTransform2 * localPoint2;
                        entity->addHitPoint(Vector3(worldPoint1.x, worldPoint1.y, worldPoint1.z));
                        entity->addHitPoint(Vector3(worldPoint2.x, worldPoint2.y, worldPoint2.z));
                    }
                }
            }
 
        private:
            CollisionResponse& collision;
    };
    // do the test
    CustomCollisionCallback customCollisionCallback(collision);
    world->testCollision(body1->rigidBody, body2->rigidBody, customCollisionCallback);
    return collision.getEntityCount() > 0;
}
 
World* World::clone(const string& id, uint16_t collisionTypeIds)
{
    //
    auto clonedWorld = new World(id);
    for (auto body: bodies) {
        // clone obv
        Body* clonedBody = nullptr;
        auto bodyType = body->getType();
 
        // test type
        if ((body->getCollisionTypeId() & collisionTypeIds) == 0) continue;
 
        // clone rigid body
        switch (bodyType) {
            case Body::BODYTYPE_STATIC:
                clonedBody = clonedWorld->addStaticRigidBody(body->id, body->getCollisionTypeId(), body->isEnabled(), body->transform, body->getFriction(), body->boundingVolumes);
                break;
            case Body::BODYTYPE_DYNAMIC:
                clonedBody = clonedWorld->addRigidBody(body->id, body->getCollisionTypeId(), body->isEnabled(), body->transform, body->getRestitution(), body->getFriction(), body->getMass(), body->inertiaTensor, body->boundingVolumes);
                break;
            case Body::BODYTYPE_COLLISION_STATIC:
                clonedBody = clonedWorld->addStaticCollisionBody(body->id, body->getCollisionTypeId(), body->isEnabled(), body->transform, body->boundingVolumes);
                break;
            case Body::BODYTYPE_COLLISION_DYNAMIC:
                clonedBody = clonedWorld->addDynamicCollisionBody(body->id, body->getCollisionTypeId(), body->isEnabled(), body->transform, body->boundingVolumes);
                break;
            default:
                Console::println("World::clone(): Unsupported type: " + to_string(bodyType));
                continue;
        }
 
        // synch additional properties
        synchronize(clonedBody, body);
    }
 
    //
    return clonedWorld;
}
 
void World::synchronize(Body* clonedBody, Body* body)
{
    clonedBody->setCollisionTypeIds(body->getCollisionTypeIds());
    clonedBody->setEnabled(body->isEnabled());
    clonedBody->setMass(body->getMass());
    clonedBody->setTransform(body->transform);
    if (clonedBody->getType() == Body::BODYTYPE_DYNAMIC) {
        clonedBody->setLinearVelocity(body->getLinearVelocity());
        clonedBody->setAngularVelocity(body->getAngularVelocity());
    }
}
 
void World::synchronize(World* world)
{
    for (auto i = 0; i < rigidBodiesDynamic.size(); i++) {
        auto body = rigidBodiesDynamic.at(i);
        auto clonedBody = world->getBody(body->id);
        if (clonedBody == nullptr) {
            Console::println("Cloned world::entity '" + body->id + "' not found");
            continue;
        }
        // synch rigid bodies
        synchronize(clonedBody, body);
    }
}
 
void World::addWorldListener(WorldListener* listener)
{
    worldListeners.push_back(listener);
}
 
void World::removeWorldListener(WorldListener* listener)
{
    worldListeners.erase(remove(worldListeners.begin(), worldListeners.end(), listener), worldListeners.end());
}