// Copyright (c) 2012-2023 Wojciech Figat. All rights reserved.

#include "Cloth.h"
#include "Engine/Core/Log.h"
#include "Engine/Core/Math/Ray.h"
#include "Engine/Graphics/Models/MeshBase.h"
#include "Engine/Graphics/Models/MeshDeformation.h"
#include "Engine/Physics/PhysicsBackend.h"
#include "Engine/Physics/PhysicsScene.h"
#include "Engine/Profiler/ProfilerCPU.h"
#include "Engine/Serialization/Serialization.h"
#include "Engine/Level/Actors/AnimatedModel.h"
#if USE_EDITOR
#include "Engine/Level/Scene/SceneRendering.h"
#include "Engine/Debug/DebugDraw.h"
#endif

Cloth::Cloth(const SpawnParams& params)
    : Actor(params)
{
    // Use the first mesh by default
    _mesh.LODIndex = _mesh.MeshIndex = 0;
}

ModelInstanceActor::MeshReference Cloth::GetMesh() const
{
    auto value = _mesh;
    value.Actor = Cast<ModelInstanceActor>(GetParent()); // Force to use cloth's parent only
    return value;
}

void Cloth::SetMesh(const ModelInstanceActor::MeshReference& value)
{
    if (_mesh.LODIndex == value.LODIndex && _mesh.MeshIndex == value.MeshIndex)
        return;

    // Remove mesh deformer (mesh index/lod changes)
    if (_meshDeformation)
    {
        Function<void(const MeshBase*, MeshDeformationData&)> deformer;
        deformer.Bind<Cloth, &Cloth::RunClothDeformer>(this);
        _meshDeformation->RemoveDeformer(_mesh.LODIndex, _mesh.MeshIndex, MeshBufferType::Vertex0, deformer);
        _meshDeformation = nullptr;
    }

    _mesh = value;
    _mesh.Actor = nullptr; // Don't store this reference
    Rebuild();
}

void Cloth::SetForce(const ForceSettings& value)
{
    _forceSettings = value;
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::SetClothForceSettings(_cloth, &value);
#endif
}

void Cloth::SetCollision(const CollisionSettings& value)
{
    _collisionSettings = value;
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::SetClothCollisionSettings(_cloth, &value);
#endif
}

void Cloth::SetSimulation(const SimulationSettings& value)
{
    _simulationSettings = value;
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::SetClothSimulationSettings(_cloth, &value);
#endif
}

void Cloth::SetFabric(const FabricSettings& value)
{
    _fabricSettings = value;
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::SetClothFabricSettings(_cloth, &value);
#endif
}

void Cloth::Rebuild()
{
#if WITH_CLOTH
    if (_cloth)
    {
        // Remove old
        if (IsDuringPlay())
            PhysicsBackend::RemoveCloth(GetPhysicsScene()->GetPhysicsScene(), _cloth);
        DestroyCloth();

        // Create new
        CreateCloth();
        if (IsDuringPlay())
            PhysicsBackend::AddCloth(GetPhysicsScene()->GetPhysicsScene(), _cloth);
    }
#endif
}

void Cloth::ClearInteria()
{
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::ClearClothInertia(_cloth);
#endif
}

Array<Float3> Cloth::GetParticles() const
{
    Array<Float3> result;
#if WITH_CLOTH
    if (_cloth)
    {
        PROFILE_CPU();
        PhysicsBackend::LockClothParticles(_cloth);
        const Span<const Float4> particles = PhysicsBackend::GetClothParticles(_cloth);
        result.Resize(particles.Length());
        const Float4* src = particles.Get();
        Float3* dst = result.Get();
        for (int32 i = 0; i < particles.Length(); i++)
            dst[i] = Float3(src[i]);
        PhysicsBackend::UnlockClothParticles(_cloth);
    }
#endif
    return result;
}

void Cloth::SetParticles(Span<const Float3> value)
{
    PROFILE_CPU();
#if !BUILD_RELEASE
    {
        // Sanity check
        const Float3* src = value.Get();
        bool allValid = true;
        for (int32 i = 0; i < value.Length(); i++)
            allValid &= !src[i].IsNanOrInfinity();
        ASSERT(allValid);
    }
#endif
#if WITH_CLOTH
    if (_cloth)
    {
        // Update cloth particles
        PhysicsBackend::LockClothParticles(_cloth);
        PhysicsBackend::SetClothParticles(_cloth, Span<const Float4>(), value, Span<const float>());
        PhysicsBackend::UnlockClothParticles(_cloth);
    }
#endif
}

Span<float> Cloth::GetPaint() const
{
    return ToSpan(_paint);
}

void Cloth::SetPaint(Span<const float> value)
{
    PROFILE_CPU();
    if (value.IsInvalid())
    {
        // Remove paint when set to empty
        _paint.SetCapacity(0);
#if WITH_CLOTH
        if (_cloth)
        {
            PhysicsBackend::SetClothPaint(_cloth, value);
        }
#endif
        return;
    }
#if !BUILD_RELEASE
    {
        // Sanity check
        const float* src = value.Get();
        bool allValid = true;
        for (int32 i = 0; i < value.Length(); i++)
            allValid &= !isnan(src[i]) && !isinf(src[i]);
        ASSERT(allValid);
    }
#endif
    _paint.Set(value.Get(), value.Length());
#if WITH_CLOTH
    if (_cloth)
    {
        // Update cloth particles
        Array<float> invMasses;
        CalculateInvMasses(invMasses);
        PhysicsBackend::LockClothParticles(_cloth);
        PhysicsBackend::SetClothParticles(_cloth, Span<const Float4>(), Span<const Float3>(), ToSpan<float, const float>(invMasses));
        PhysicsBackend::UnlockClothParticles(_cloth);
        PhysicsBackend::SetClothPaint(_cloth, value);
    }
#endif
}

bool Cloth::IntersectsItself(const Ray& ray, Real& distance, Vector3& normal)
{
#if USE_PRECISE_MESH_INTERSECTS
    if (!Actor::IntersectsItself(ray, distance, normal))
        return false;
#if WITH_CLOTH
    if (_cloth)
    {
        // Precise per-triangle intersection
        const ModelInstanceActor::MeshReference mesh = GetMesh();
        if (mesh.Actor == nullptr)
            return false;
        BytesContainer indicesData;
        int32 indicesCount;
        if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Index, indicesData, indicesCount))
            return false;
        PhysicsBackend::LockClothParticles(_cloth);
        const Span<const Float4> particles = PhysicsBackend::GetClothParticles(_cloth);
        const Transform transform = GetTransform();
        const bool indices16bit = indicesData.Length() / indicesCount == sizeof(uint16);
        const int32 trianglesCount = indicesCount / 3;
        bool result = false;
        distance = MAX_Real;
        for (int32 triangleIndex = 0; triangleIndex < trianglesCount; triangleIndex++)
        {
            const int32 index = triangleIndex * 3;
            int32 i0, i1, i2;
            if (indices16bit)
            {
                i0 = indicesData.Get<uint16>()[index];
                i1 = indicesData.Get<uint16>()[index + 1];
                i2 = indicesData.Get<uint16>()[index + 2];
            }
            else
            {
                i0 = indicesData.Get<uint32>()[index];
                i1 = indicesData.Get<uint32>()[index + 1];
                i2 = indicesData.Get<uint32>()[index + 2];
            }
            const Vector3 v0 = transform.LocalToWorld(Vector3(particles[i0]));
            const Vector3 v1 = transform.LocalToWorld(Vector3(particles[i1]));
            const Vector3 v2 = transform.LocalToWorld(Vector3(particles[i2]));
            Real d;
            if (CollisionsHelper::RayIntersectsTriangle(ray, v0, v1, v2, d) && d < distance)
            {
                result = true;
                normal = Vector3::Normalize((v1 - v0) ^ (v2 - v0));
                distance = d;

                // Flip normal if needed as cloth is two-sided
                const Vector3 hitPos = ray.GetPoint(d);
                if (Vector3::DistanceSquared(hitPos + normal, ray.Position) > Math::Square(d))
                    normal = -normal;
            }
        }
        PhysicsBackend::UnlockClothParticles(_cloth);
        return result;
    }
#endif
    return false;
#else
    return Actor::IntersectsItself(ray, distance, normal);
#endif
}

void Cloth::Serialize(SerializeStream& stream, const void* otherObj)
{
    Actor::Serialize(stream, otherObj);

    SERIALIZE_GET_OTHER_OBJ(Cloth);

    SERIALIZE_MEMBER(Mesh, _mesh);
    SERIALIZE_MEMBER(Force, _forceSettings);
    SERIALIZE_MEMBER(Collision, _collisionSettings);
    SERIALIZE_MEMBER(Simulation, _simulationSettings);
    SERIALIZE_MEMBER(Fabric, _fabricSettings);
    if (Serialization::ShouldSerialize(_paint, other ? &other->_paint : nullptr))
    {
        // Serialize as Base64
        stream.JKEY("Paint");
        stream.Blob(_paint.Get(), _paint.Count() * sizeof(float));
    }
}

void Cloth::Deserialize(DeserializeStream& stream, ISerializeModifier* modifier)
{
    Actor::Deserialize(stream, modifier);

    DESERIALIZE_MEMBER(Mesh, _mesh);
    _mesh.Actor = nullptr; // Don't store this reference
    DESERIALIZE_MEMBER(Force, _forceSettings);
    DESERIALIZE_MEMBER(Collision, _collisionSettings);
    DESERIALIZE_MEMBER(Simulation, _simulationSettings);
    DESERIALIZE_MEMBER(Fabric, _fabricSettings);
    DESERIALIZE_MEMBER(Paint, _paint);

    // Refresh cloth when settings were changed
    if (IsDuringPlay())
        Rebuild();
}

#if USE_EDITOR

void Cloth::DrawPhysicsDebug(RenderView& view)
{
#if WITH_CLOTH && COMPILE_WITH_DEBUG_DRAW
    if (_cloth)
    {
        const ModelInstanceActor::MeshReference mesh = GetMesh();
        if (mesh.Actor == nullptr)
            return;
        BytesContainer indicesData;
        int32 indicesCount;
        if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Index, indicesData, indicesCount))
            return;
        PhysicsBackend::LockClothParticles(_cloth);
        const Span<const Float4> particles = PhysicsBackend::GetClothParticles(_cloth);
        const Transform transform = GetTransform();
        const bool indices16bit = indicesData.Length() / indicesCount == sizeof(uint16);
        const int32 trianglesCount = indicesCount / 3;
        for (int32 triangleIndex = 0; triangleIndex < trianglesCount; triangleIndex++)
        {
            const int32 index = triangleIndex * 3;
            int32 i0, i1, i2;
            if (indices16bit)
            {
                i0 = indicesData.Get<uint16>()[index];
                i1 = indicesData.Get<uint16>()[index + 1];
                i2 = indicesData.Get<uint16>()[index + 2];
            }
            else
            {
                i0 = indicesData.Get<uint32>()[index];
                i1 = indicesData.Get<uint32>()[index + 1];
                i2 = indicesData.Get<uint32>()[index + 2];
            }
            const Vector3 v0 = transform.LocalToWorld(Vector3(particles[i0]));
            const Vector3 v1 = transform.LocalToWorld(Vector3(particles[i1]));
            const Vector3 v2 = transform.LocalToWorld(Vector3(particles[i2]));
            DEBUG_DRAW_TRIANGLE(v0, v1, v2, Color::Pink, 0, true);
        }
        PhysicsBackend::UnlockClothParticles(_cloth);
    }
#endif
}

void Cloth::OnDebugDrawSelected()
{
#if WITH_CLOTH && COMPILE_WITH_DEBUG_DRAW
    if (_cloth)
    {
        DEBUG_DRAW_WIRE_BOX(_box, Color::Violet.RGBMultiplied(0.8f), 0, true);
        const ModelInstanceActor::MeshReference mesh = GetMesh();
        if (mesh.Actor == nullptr)
            return;
        BytesContainer indicesData;
        int32 indicesCount;
        if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Index, indicesData, indicesCount))
            return;
        PhysicsBackend::LockClothParticles(_cloth);
        const Span<const Float4> particles = PhysicsBackend::GetClothParticles(_cloth);
        const Transform transform = GetTransform();
        const bool indices16bit = indicesData.Length() / indicesCount == sizeof(uint16);
        const int32 trianglesCount = indicesCount / 3;
        for (int32 triangleIndex = 0; triangleIndex < trianglesCount; triangleIndex++)
        {
            const int32 index = triangleIndex * 3;
            int32 i0, i1, i2;
            if (indices16bit)
            {
                i0 = indicesData.Get<uint16>()[index];
                i1 = indicesData.Get<uint16>()[index + 1];
                i2 = indicesData.Get<uint16>()[index + 2];
            }
            else
            {
                i0 = indicesData.Get<uint32>()[index];
                i1 = indicesData.Get<uint32>()[index + 1];
                i2 = indicesData.Get<uint32>()[index + 2];
            }
            const Vector3 v0 = transform.LocalToWorld(Vector3(particles[i0]));
            const Vector3 v1 = transform.LocalToWorld(Vector3(particles[i1]));
            const Vector3 v2 = transform.LocalToWorld(Vector3(particles[i2]));
            Color c0 = Color::White, c1 = Color::White, c2 = Color::White;
            if (_paint.Count() == particles.Length())
            {
                c0 = Color::Lerp(Color::Red, Color::White, _paint[i0]);
                c1 = Color::Lerp(Color::Red, Color::White, _paint[i1]);
                c2 = Color::Lerp(Color::Red, Color::White, _paint[i2]);
            }
            DebugDraw::DrawLine(v0, v1, c0, c1, 0, false);
            DebugDraw::DrawLine(v1, v2, c1, c2, 0, false);
            DebugDraw::DrawLine(v2, v0, c2, c0, 0, false);
        }
        PhysicsBackend::UnlockClothParticles(_cloth);
    }
#endif

    Actor::OnDebugDrawSelected();
}

#endif

void Cloth::BeginPlay(SceneBeginData* data)
{
#if WITH_CLOTH
    if (CreateCloth())
        LOG(Error, "Failed to create cloth '{0}'", GetNamePath());

#endif

    Actor::BeginPlay(data);
}

void Cloth::EndPlay()
{
    Actor::EndPlay();

#if WITH_CLOTH
    if (_cloth)
        DestroyCloth();
#endif
}

void Cloth::OnEnable()
{
#if USE_EDITOR
    GetSceneRendering()->AddPhysicsDebug<Cloth, &Cloth::DrawPhysicsDebug>(this);
#endif
#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::AddCloth(GetPhysicsScene()->GetPhysicsScene(), _cloth);
#endif

    Actor::OnEnable();
}

void Cloth::OnDisable()
{
    Actor::OnDisable();

#if WITH_CLOTH
    if (_cloth)
        PhysicsBackend::RemoveCloth(GetPhysicsScene()->GetPhysicsScene(), _cloth);
#endif
#if USE_EDITOR
    GetSceneRendering()->RemovePhysicsDebug<Cloth, &Cloth::DrawPhysicsDebug>(this);
#endif
}

void Cloth::OnParentChanged()
{
    Actor::OnParentChanged();

    Rebuild();
}

void Cloth::OnTransformChanged()
{
    Actor::OnTransformChanged();

#if WITH_CLOTH
    if (_cloth)
    {
        // Move cloth but consider this as teleport if the position delta is significant
        const float minTeleportDistanceSq = Math::Square(1000.0f);
        const bool teleport = Vector3::DistanceSquared(_cachedPosition, _transform.Translation) >= minTeleportDistanceSq;
        _cachedPosition = _transform.Translation;
        PhysicsBackend::SetClothTransform(_cloth, _transform, teleport);
    }
    else
#endif
    {
        _box = BoundingBox(_transform.Translation);
        _sphere = BoundingSphere(_transform.Translation, 0.0f);
    }
}

void Cloth::OnPhysicsSceneChanged(PhysicsScene* previous)
{
    Actor::OnPhysicsSceneChanged(previous);

#if WITH_CLOTH
    if (_cloth)
    {
        PhysicsBackend::RemoveCloth(previous->GetPhysicsScene(), _cloth);
        void* scene = GetPhysicsScene()->GetPhysicsScene();
        PhysicsBackend::AddCloth(scene, _cloth);
    }
#endif
}

bool Cloth::CreateCloth()
{
#if WITH_CLOTH
    PROFILE_CPU();

    // Get mesh data
    // TODO: consider making it via async task so physics can wait on the cloth setup from mesh data just before next fixed update which gives more time when loading scene
    const ModelInstanceActor::MeshReference mesh = GetMesh();
    if (mesh.Actor == nullptr)
        return false;
    PhysicsClothDesc desc;
    desc.Actor = this;
    BytesContainer data;
    int32 count;
    if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Vertex0, data, count))
        return true;
    desc.VerticesData = data.Get();
    desc.VerticesCount = count;
    desc.VerticesStride = data.Length() / count;
    if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Index, data, count))
        return true;
    desc.IndicesData = data.Get();
    desc.IndicesCount = count;
    desc.IndicesStride = data.Length() / count;
    Array<float> invMasses;
    CalculateInvMasses(invMasses);
    desc.InvMassesData = invMasses.Count() == desc.VerticesCount ? invMasses.Get() : nullptr;
    desc.InvMassesStride = sizeof(float);
    desc.MaxDistancesData = _paint.Count() == desc.VerticesCount ? _paint.Get() : nullptr;
    desc.MaxDistancesStride = sizeof(float);

    // Create cloth
    ASSERT(_cloth == nullptr);
    _cloth = PhysicsBackend::CreateCloth(desc);
    if (_cloth == nullptr)
        return true;
    _cachedPosition = _transform.Translation;
    PhysicsBackend::SetClothForceSettings(_cloth, &_forceSettings);
    PhysicsBackend::SetClothCollisionSettings(_cloth, &_collisionSettings);
    PhysicsBackend::SetClothSimulationSettings(_cloth, &_simulationSettings);
    PhysicsBackend::SetClothFabricSettings(_cloth, &_fabricSettings);
    PhysicsBackend::SetClothTransform(_cloth, _transform, true);
    PhysicsBackend::ClearClothInertia(_cloth);

    // Add cloth mesh deformer
    if (auto* deformation = mesh.Actor->GetMeshDeformation())
    {
        Function<void(const MeshBase*, MeshDeformationData&)> deformer;
        deformer.Bind<Cloth, &Cloth::RunClothDeformer>(this);
        deformation->AddDeformer(mesh.LODIndex, mesh.MeshIndex, MeshBufferType::Vertex0, deformer);
        _meshDeformation = deformation;
    }
#endif

    return false;
}

void Cloth::DestroyCloth()
{
#if WITH_CLOTH
    if (_meshDeformation)
    {
        Function<void(const MeshBase*, MeshDeformationData&)> deformer;
        deformer.Bind<Cloth, &Cloth::RunClothDeformer>(this);
        _meshDeformation->RemoveDeformer(_mesh.LODIndex, _mesh.MeshIndex, MeshBufferType::Vertex0, deformer);
        _meshDeformation = nullptr;
    }
    PhysicsBackend::DestroyCloth(_cloth);
    _cloth = nullptr;
#endif
}

void Cloth::CalculateInvMasses(Array<float>& invMasses)
{
    // Use per-particle max distance to evaluate which particles are immovable
#if WITH_CLOTH
    if (_paint.IsEmpty())
        return;

    // Get mesh data
    const ModelInstanceActor::MeshReference mesh = GetMesh();
    if (mesh.Actor == nullptr)
        return;
    BytesContainer verticesData;
    int32 verticesCount;
    if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Vertex0, verticesData, verticesCount))
        return;
    BytesContainer indicesData;
    int32 indicesCount;
    if (mesh.Actor->GetMeshData(mesh, MeshBufferType::Index, indicesData, indicesCount))
        return;
    const int32 verticesStride = verticesData.Length() / verticesCount;
    const bool indices16bit = indicesData.Length() / indicesCount == sizeof(uint16);
    const int32 trianglesCount = indicesCount / 3;

    // Sum triangle area for each influenced particle
    invMasses.Resize(verticesCount);
    for (int32 triangleIndex = 0; triangleIndex < trianglesCount; triangleIndex++)
    {
        const int32 index = triangleIndex * 3;
        int32 i0, i1, i2;
        if (indices16bit)
        {
            i0 = indicesData.Get<uint16>()[index];
            i1 = indicesData.Get<uint16>()[index + 1];
            i2 = indicesData.Get<uint16>()[index + 2];
        }
        else
        {
            i0 = indicesData.Get<uint32>()[index];
            i1 = indicesData.Get<uint32>()[index + 1];
            i2 = indicesData.Get<uint32>()[index + 2];
        }
#define GET_POS(i) *(Float3*)((byte*)verticesData.Get() + i * verticesStride)
        const Float3 v0(GET_POS(i0));
        const Float3 v1(GET_POS(i1));
        const Float3 v2(GET_POS(i2));
#undef GET_POS
        const float area = Float3::TriangleArea(v0, v1, v2);
        invMasses.Get()[i0] += area;
        invMasses.Get()[i1] += area;
        invMasses.Get()[i2] += area;
    }

    // Count fixed vertices which max movement distance is zero
    int32 fixedCount = 0;
    float massSum = 0;
    for (int32 i = 0; i < verticesCount; i++)
    {
        float& mass = invMasses[i];
        const float maxDistance = _paint[i];
        if (maxDistance < 0.01f)
        {
            // Fixed
            fixedCount++;
            mass = 0.0f;
        }
        else
        {
            // Kinetic so include it's mass contribution
            massSum += mass;
        }
    }

    if (massSum > ZeroTolerance)
    {
        // Normalize and inverse particles mass
        const float massScale = (float)(verticesCount - fixedCount) / massSum;
        for (int32 i = 0; i < verticesCount; i++)
        {
            float& mass = invMasses[i];
            if (mass > 0.0f)
            {
                mass *= massScale;
                mass = 1.0f / mass;
            }
        }
    }
#endif
}

void Cloth::OnUpdated()
{
    if (_meshDeformation)
    {
        // Mark mesh as dirty
        const Matrix invWorld = Matrix::Invert(_transform.GetWorld());
        BoundingBox localBounds;
        BoundingBox::Transform(_box, invWorld, localBounds);
        _meshDeformation->Dirty(_mesh.LODIndex, _mesh.MeshIndex, MeshBufferType::Vertex0, localBounds);

        // Update bounds (for mesh culling)
        auto* actor = (ModelInstanceActor*)GetParent();
        actor->UpdateBounds();
    }
}

void Cloth::RunClothDeformer(const MeshBase* mesh, MeshDeformationData& deformation)
{
#if WITH_CLOTH
    PROFILE_CPU_NAMED("Cloth");
    PhysicsBackend::LockClothParticles(_cloth);
    const Span<const Float4> particles = PhysicsBackend::GetClothParticles(_cloth);

    // Update mesh vertices based on the cloth particles positions
    auto vbData = deformation.VertexBuffer.Data.Get();
    auto vbCount = (uint32)mesh->GetVertexCount();
    auto vbStride = (uint32)deformation.VertexBuffer.Data.Count() / vbCount;
    // TODO: add support for mesh vertex data layout descriptor instead hardcoded position data at the beginning of VB0
    ASSERT((uint32)particles.Length() >= vbCount);
    if (auto* animatedModel = Cast<AnimatedModel>(GetParent()))
    {
        if (animatedModel->GraphInstance.NodesPose.IsEmpty())
        {
            // Delay unit skinning data is ready
            PhysicsBackend::UnlockClothParticles(_cloth);
            _meshDeformation->Dirty(_mesh.LODIndex, _mesh.MeshIndex, MeshBufferType::Vertex0);
            return;
        }

        // TODO: optimize memory allocs (eg. get pose as Span<Matrix> for readonly)
        Array<Matrix> pose;
        animatedModel->GetCurrentPose(pose);
        const SkeletonData& skeleton = animatedModel->SkinnedModel->Skeleton;

        // Animated model uses skinning thus requires to set vertex position inverse to skeleton bones
        ASSERT(vbStride == sizeof(VB0SkinnedElementType));
        for (uint32 i = 0; i < vbCount; i++)
        {
            VB0SkinnedElementType& vb0 = *(VB0SkinnedElementType*)vbData;

            // Calculate skinned vertex matrix from bones blending
            const Float4 blendWeights = vb0.BlendWeights.ToFloat4();
            // TODO: optimize this or use _skinningData from AnimatedModel to access current mesh bones data directly
            Matrix matrix;
            const SkeletonBone& bone0 = skeleton.Bones[vb0.BlendIndices.R];
            Matrix::Multiply(bone0.OffsetMatrix, pose[bone0.NodeIndex], matrix);
            Matrix boneMatrix = matrix * blendWeights.X;
            if (blendWeights.Y > 0.0f)
            {
                const SkeletonBone& bone1 = skeleton.Bones[vb0.BlendIndices.G];
                Matrix::Multiply(bone1.OffsetMatrix, pose[bone1.NodeIndex], matrix);
                boneMatrix += matrix * blendWeights.Y;
            }
            if (blendWeights.Z > 0.0f)
            {
                const SkeletonBone& bone2 = skeleton.Bones[vb0.BlendIndices.B];
                Matrix::Multiply(bone2.OffsetMatrix, pose[bone2.NodeIndex], matrix);
                boneMatrix += matrix * blendWeights.Z;
            }
            if (blendWeights.W > 0.0f)
            {
                const SkeletonBone& bone3 = skeleton.Bones[vb0.BlendIndices.A];
                Matrix::Multiply(bone3.OffsetMatrix, pose[bone3.NodeIndex], matrix);
                boneMatrix += matrix * blendWeights.W;
            }

            // Set vertex position so it will match cloth particle pos after skinning with bone matrix
            Matrix boneMatrixInv;
            Matrix::Invert(boneMatrix, boneMatrixInv);
            Float3 pos = *(Float3*)&particles.Get()[i];
            vb0.Position = Float3::Transform(pos, boneMatrixInv);

            vbData += vbStride;
        }
    }
    else
    {
        for (uint32 i = 0; i < vbCount; i++)
        {
            *(Float3*)vbData = *(Float3*)&particles.Get()[i];
            vbData += vbStride;
        }
    }

    // Mark whole mesh as modified
    deformation.DirtyMinIndex = 0;
    deformation.DirtyMaxIndex = vbCount;

    PhysicsBackend::UnlockClothParticles(_cloth);
#endif
}