here is the metal code and  original in cuda, I am doing in metal shading language and it doesn;t work. please review and fix it. method summary - Vertex Block Descent (VBD) for physics-based simulation, focusing on elastic body dynamics. It introduces a block-coordinate descent method using vertex-level Gauss-Seidel iterations to solve the variational form of implicit Euler integration. VBD operates by updating vertex positions to minimize global variational energy, allowing for high levels of parallelism. It guarantees unconditional stability and fast convergence, especially for complex simulations like elastic bodies, particle systems, and rigid bodies.
Main Equations:
Global Optimization Problem:
x
t
+
1
=
arg
⁡
min
⁡
x
G
(
x
)
x 
t+1
 =arg 
x
min
​	
 G(x)
Where 
G
(
x
)
G(x) represents the global variational energy.
Variational Energy:
G
(
x
)
=
1
2
h
2
∥
x
−
y
∥
M
2
+
E
(
x
)
G(x)= 
2h 
2
 
1
​	
 ∥x−y∥ 
M
2
​	
 +E(x)
Where 
y
=
x
t
+
h
v
t
+
h
2
a
e
x
t
y=x 
t
 +hv 
t
 +h 
2
 a 
ext
​	
 , and 
E
(
x
)
E(x) is the potential energy of the system.
Local Energy Minimization: For each vertex 
i
i, the local variational energy is:
G
i
(
x
)
=
m
i
2
h
2
∥
x
i
−
y
i
∥
2
+
∑
j
∈
F
i
E
j
(
x
)
G 
i
​	
 (x)= 
2h 
2
 
m 
i
​	
 
​	
 ∥x 
i
​	
 −y 
i
​	
 ∥ 
2
 + 
j∈F 
i
​	
 
∑
​	
 E 
j
​	
 (x)
The update step for vertex 
i
i:
x
i
←
arg
⁡
min
⁡
x
i
G
i
(
x
)
x 
i
​	
 ←arg 
x 
i
​	
 
min
​	
 G 
i
​	
 (x)
Newton’s Method for Local System:
H
i
Δ
x
i
=
f
i
H 
i
​	
 Δx 
i
​	
 =f 
i
​	
 
Where 
H
i
H 
i
​	
  is the Hessian, and 
f
i
f 
i
​	
  is the total force on vertex 
i
i.
Velocity Update:
v
t
+
1
=
1
h
(
x
t
+
1
−
x
t
)
v 
t+1
 = 
h
1
​	
 (x 
t+1
 −x 
t
 )


struct Mat9x9Static {
    float data[81];

    METAL_FUNC thread float& operator()(int row, int col) {
        return data[col * 9 + row];
    }

    METAL_FUNC const thread float& operator()(int row, int col) const {
        return data[col * 9 + row];
    }

    METAL_FUNC void multiplyBy(float scalar) {
        for (int i = 0; i < 81; ++i) {
            data[i] *= scalar;
        }
    }

    // Initialize to zero
    METAL_FUNC void initializeZero() {
        for (int i = 0; i < 81; ++i) {
            data[i] = 0.0f;
        }
    }
};

METAL_FUNC void vec9OuterProduct(const thread float* a, const thread float* b, thread Mat9x9Static& result) {
    for (int i = 0; i < 9; ++i) {
        for (int j = 0; j < 9; ++j) {
            result(i, j) = a[i] * b[j];
        }
    }
}

METAL_FUNC void vec9Mul(const thread float* a, float scalar, thread float* result) {
    for (int i = 0; i < 9; ++i) {
        result[i] = a[i] * scalar;
    }
}

METAL_FUNC void vec9Add(const thread float* a, const thread float* b, thread float* result) {
    for (int i = 0; i < 9; ++i) {
        result[i] = a[i] + b[i];
    }
}

METAL_FUNC void assembleVertexVForceAndHessian(const thread float* dE_dF, const thread Mat9x9Static& d2E_dF_dF,
                                               float m1, float m2, float m3,
                                               thread float3& force, thread float* h) {
    // Extract components of dE_dF for easier access
    float A1 = dE_dF[0];
    float A2 = dE_dF[1];
    float A3 = dE_dF[2];
    float A4 = dE_dF[3];
    float A5 = dE_dF[4];
    float A6 = dE_dF[5];
    float A7 = dE_dF[6];
    float A8 = dE_dF[7];
    float A9 = dE_dF[8];

    // Compute the force components
    // Note: The force is the negative gradient of the energy
    force.x -= A1 * m1 + A4 * m2 + A7 * m3;
    force.y -= A2 * m1 + A5 * m2 + A8 * m3;
    force.z -= A3 * m1 + A6 * m2 + A9 * m3;

    // Initialize temporary storage for Hessian computation
    float HL_Row1[9], HL_Row2[9], HL_Row3[9];
    thread float* HL[3] = { HL_Row1, HL_Row2, HL_Row3 };

    // Compute the product of d2E_dF_dF with the shape function derivatives
    for (int iCol = 0; iCol < 9; iCol++) {
        HL_Row1[iCol] = d2E_dF_dF(0, iCol) * m1 + d2E_dF_dF(3, iCol) * m2 + d2E_dF_dF(6, iCol) * m3;
        HL_Row2[iCol] = d2E_dF_dF(1, iCol) * m1 + d2E_dF_dF(4, iCol) * m2 + d2E_dF_dF(7, iCol) * m3;
        HL_Row3[iCol] = d2E_dF_dF(2, iCol) * m1 + d2E_dF_dF(5, iCol) * m2 + d2E_dF_dF(8, iCol) * m3;
    }

    // Assemble the final Hessian
    for (int iRow = 0; iRow < 3; iRow++) {
        // Compute the diagonal elements of the Hessian
        h[iRow] += HL[iRow][0] * m1 + HL[iRow][3] * m2 + HL[iRow][6] * m3;
        // Compute the off-diagonal elements of the Hessian
        h[iRow + 3] += HL[iRow][1] * m1 + HL[iRow][4] * m2 + HL[iRow][7] * m3;
        h[iRow + 6] += HL[iRow][2] * m1 + HL[iRow][5] * m2 + HL[iRow][8] * m3;
    }
}

METAL_FUNC void assembleDeterminantHessian(const thread float* ddetF_dF, thread Mat9x9Static& d2DetF_dF_dF) {
    // Compute the second derivative of detF
    // d2(detF)/dF_ij dF_kl = Cof(F)_jk * delta_il - Cof(F)_jl * delta_ik
    for(int i = 0; i < 3; ++i) {          // i corresponds to row in F
        for(int j = 0; j < 3; ++j) {      // j corresponds to column in F
            for(int k = 0; k < 3; ++k) {  // k corresponds to row in second F
                for(int l = 0; l < 3; ++l) { // l corresponds to column in second F
                    int row = i * 3 + j;
                    int col = k * 3 + l;
                    float delta_il = (i == l) ? 1.0f : 0.0f;
                    float delta_ik = (i == k) ? 1.0f : 0.0f;
                    float value = ddetF_dF[j * 3 + k] * delta_il - ddetF_dF[j * 3 + l] * delta_ik;
                    d2DetF_dF_dF(row, col) += value;
                }
            }
        }
    }
}

METAL_FUNC void evaluateNeoHookeanMaterialForceAndHessian(float miu, float lmbd, float volume,
                                                          int vOrderInTet, const uint4 tetVIds, constant float3* vs,
                                                          const constant float3* vsPrev,
                                                          float3x3 DmInv,
                                                          thread float3& force,
                                                          thread float* h,
                                                          float dampingHydrostatic,
                                                          float dampingDeviatoric,
                                                          float dt)
{
    // Retrieve the positions of the four vertices of the tetrahedron
    float3 v0 = vs[int(tetVIds[0])];
    float3 v1 = vs[int(tetVIds[1])];
    float3 v2 = vs[int(tetVIds[2])];
    float3 v3 = vs[int(tetVIds[3])];

    // Compute the deformation matrix Ds
    float3x3 Ds;
    Ds[0] = v1 - v0;  // Edge vector from v0 to v1
    Ds[1] = v2 - v0;  // Edge vector from v0 to v2
    Ds[2] = v3 - v0;  // Edge vector from v0 to v3

    // Compute the deformation gradient F
    float3x3 F = Ds * DmInv;

    // Compute the 'a' parameter for the Neo-Hookean model
    float a = 1 + miu / lmbd;
    // Compute the determinant of F, which represents the volume change
    float detF = determinant(F);

    // Create a 1D array representation of F for easier access
    float F_array[9] = {F[0][0], F[0][1], F[0][2],
                        F[1][0], F[1][1], F[1][2],
                        F[2][0], F[2][1], F[2][2]};

    // Store individual components of F for later use
    float F1_1 = F[0][0], F2_1 = F[1][0], F3_1 = F[2][0];
    float F1_2 = F[0][1], F2_2 = F[1][1], F3_2 = F[2][1];
    float F1_3 = F[0][2], F2_3 = F[1][2], F3_3 = F[2][2];

    // Compute the derivative of detF with respect to F (cofactor matrix)
    float ddetF_dF[9] = {
        F2_2 * F3_3 - F2_3 * F3_2,
        F1_3 * F3_2 - F1_2 * F3_3,
        F1_2 * F2_3 - F1_3 * F2_2,
        F2_3 * F3_1 - F2_1 * F3_3,
        F1_1 * F3_3 - F1_3 * F3_1,
        F1_3 * F2_1 - F1_1 * F2_3,
        F2_1 * F3_2 - F2_2 * F3_1,
        F1_2 * F3_1 - F1_1 * F3_2,
        F1_1 * F2_2 - F1_2 * F2_1
    };

    // Initialize the Hessian matrix correctly
    Mat9x9Static d2E_dF_dF;
    d2E_dF_dF.initializeZero();

    // Compute the outer product of ddetF_dF
    Mat9x9Static outerProduct;
    outerProduct.initializeZero();
    vec9OuterProduct(ddetF_dF, ddetF_dF, outerProduct);

    // Scale the outer product by lambda and add to Hessian
    outerProduct.multiplyBy(lmbd);
    for(int i = 0; i < 81; ++i) {
        d2E_dF_dF.data[i] += outerProduct.data[i];
    }

    // Compute the second derivative of detF and add it to the Hessian
    Mat9x9Static d2DetF_dF_dF;
    d2DetF_dF_dF.initializeZero();
    assembleDeterminantHessian(ddetF_dF, d2DetF_dF_dF);

    // Scale the second derivative by lambda * k and add to Hessian
    float k = detF - a;
    for(int i = 0; i < 81; ++i) {
        d2E_dF_dF.data[i] += d2DetF_dF_dF.data[i] * lmbd * k;
    }

    // Add mu to the diagonal elements of the Hessian
    for (int i = 0; i < 9; ++i) {
        d2E_dF_dF(i, i) += miu;
    }

    // Scale the Hessian by the volume
    d2E_dF_dF.multiplyBy(volume);

    // Compute the derivative of the deviatoric energy with respect to F
    float dPhi_D_dF[9];
    vec9Mul(F_array, miu, dPhi_D_dF);

    // Compute the derivative of the volumetric energy with respect to F
    float scaled_ddetF_dF[9];
    vec9Mul(ddetF_dF, lmbd * k, scaled_ddetF_dF);

    // Compute the total energy derivative
    float dE_dF[9];
    vec9Add(dPhi_D_dF, scaled_ddetF_dF, dE_dF);
    vec9Mul(dE_dF, volume, dE_dF);

    // Extract inverse rest matrix components
    float DmInv1_1 = DmInv[0][0];
    float DmInv2_1 = DmInv[1][0];
    float DmInv3_1 = DmInv[2][0];
    float DmInv1_2 = DmInv[0][1];
    float DmInv2_2 = DmInv[1][1];
    float DmInv3_2 = DmInv[2][1];
    float DmInv1_3 = DmInv[0][2];
    float DmInv2_3 = DmInv[1][2];
    float DmInv3_3 = DmInv[2][2];

    // Shape function derivatives for all vertices in the tetrahedron
    float ms[4][3] = {
        {-DmInv1_1 - DmInv2_1 - DmInv3_1, -DmInv1_2 - DmInv2_2 - DmInv3_2, -DmInv1_3 - DmInv2_3 - DmInv3_3},
        {DmInv1_1, DmInv1_2, DmInv1_3},
        {DmInv2_1, DmInv2_2, DmInv2_3},
        {DmInv3_1, DmInv3_2, DmInv3_3}
    };

    // Get the shape function derivatives for the current vertex
    float m1 = ms[vOrderInTet][0], m2 = ms[vOrderInTet][1], m3 = ms[vOrderInTet][2];

    // Assemble the force and Hessian for the current vertex
    assembleVertexVForceAndHessian(dE_dF, d2E_dF_dF, m1, m2, m3, force, h);

    // Apply damping if needed
    if (dampingHydrostatic > 0.f || dampingDeviatoric > 0.f) {
        float dampingH[9];
        vec9Mul(h, dampingHydrostatic, dampingH);

        float tmp = (m1 * m1 + m2 * m2 + m3 * m3) * miu * volume;

        h[0] += tmp;
        h[4] += tmp;
        h[8] += tmp;
        tmp *= dampingDeviatoric;
        dampingH[0] += tmp;
        dampingH[4] += tmp;
        dampingH[8] += tmp;
        vec9Mul(dampingH, 1 / dt, dampingH);

        // Compute the displacement
        float3 v = vs[tetVIds[vOrderInTet]];
        float3 vPrev = vsPrev[tetVIds[vOrderInTet]];
        float3 displacement = vPrev - v;

        // Compute the damping force
        float3 dampingForce;
        dampingForce.x = dot(float3(dampingH[0], dampingH[3], dampingH[6]), displacement);
        dampingForce.y = dot(float3(dampingH[1], dampingH[4], dampingH[7]), displacement);
        dampingForce.z = dot(float3(dampingH[2], dampingH[5], dampingH[8]), displacement);
        force += dampingForce;

        // Add damping to the Hessian
        for (int i = 0; i < 9; ++i) {
            h[i] += dampingH[i];
        }
    }
}

METAL_FUNC void reduceAndSolveVBD(
    uint simdIndex,
    uint vertexIndex,
    float3 force,
    float3x3 hessian,
    constant float3* prevPositions,
    device float3* positions,
    constant float3* preIterPositions,
    constant float3* originalPositions,
    constant float3* inertias,
    StepInfo stepInfo,
    SimulationConfigurationGPU configuration,
    float3 p)
{
    float3 totalForce = simd_sum(force);
    float3 hessianSum0 = simd_sum(hessian.columns[0]);
    float3 hessianSum1 = simd_sum(hessian.columns[1]);
    float3 hessianSum2 = simd_sum(hessian.columns[2]);

    if (simdIndex == 0) {
        float3 currentForce = totalForce;
        float3x3 currentHessian = float3x3(hessianSum0, hessianSum1,  hessianSum2);
        
        p = p.x == FLT_MAX ? preIterPositions[vertexIndex].xyz : p;
        const float3 inertiaDiff = (inertias[vertexIndex].xyz - p);
        const float mass = 1.0; //1.0 / configuration.invMass;
        const float dtSqrReciprocal = 1.0 / (stepInfo.dt * stepInfo.dt);
        
        positions[vertexIndex].xyz = p + currentForce;
        
        updatePosition(currentForce, currentHessian,
                       mass, inertiaDiff, dtSqrReciprocal,
                       prevPositions, positions, vertexIndex, p);
    }
}


kernel void solveVBDNeoHookean(
    device float3 *positions [[buffer(0)]],
    constant TetConstraintDescriptorGPU *tetDescriptors [[buffer(1)]],
    constant StepInfo &stepInfo [[buffer(2)]],
    constant SimulationConfigurationGPU &configuration [[buffer(3)]],
    constant float3 *prevPositions [[buffer(4)]],
    constant float3 *inertias [[buffer(5)]],
    constant float3 *preIterPositions [[buffer(6)]],
    constant float3 *originalPositions [[buffer(7)]],
    constant uint &descriptorsCount [[buffer(8)]],
    constant float *invMasses [[buffer(9)]],
    uint gid [[thread_position_in_grid]],
    uint simdIndex [[thread_index_in_simdgroup]],
    uint simdgroupsPerThreadgroup [[simdgroups_per_threadgroup]])
{
    if (gid >= descriptorsCount) { return; }
    
    const TetConstraintDescriptorGPU descriptor = tetDescriptors[gid];
    const uint4 tet = descriptor.indices;
    const uint vertexIndex = tet[descriptor.indexInTet];
    float3 force = float3(0);
    float3x3 hessian = float3x3(0);
    float3 p = FLT_MAX;

    if (descriptor.indices.w != UINT_MAX) {
        p = preIterPositions[int(vertexIndex)];
        const float dt = stepInfo.dt;
        const float damping = 1e-5;
        const float mu = 10000.0; // Shear modulus
        const float lambda = 10000.0; // Lamé's first parameter
        const float3x3 invRestMat = descriptor.invRestMat;
        const uint indexInTet = descriptor.indexInTet;
        const float restVolume = descriptor.inverseRestVolume; // Ensure correct usage: it's not inversed

        float h[9];
        evaluateNeoHookeanMaterialForceAndHessian(mu, lambda, restVolume, indexInTet, descriptor.indices, preIterPositions, prevPositions, invRestMat, force, h, damping, damping, dt);
        
        hessian.columns[0] = float3(h[0], h[1], h[2]);
        hessian.columns[1] = float3(h[3], h[4], h[5]);
        hessian.columns[2] = float3(h[6], h[7], h[8]);
    }
    
    reduceAndSolveVBD(simdIndex, vertexIndex, force, hessian,
                   prevPositions, positions, preIterPositions, originalPositions,
                   inertias, stepInfo, configuration, p);
}
 
GPU_CPU_INLINE_FUNC CFloatingTypeGPU GAIA::evaluateNeoHookeanEnergy(FloatingTypeGPU miu, FloatingTypeGPU lmbd, FloatingTypeGPU A, int32_t tetId,
	const int32_t* tetVIds, FloatingTypeGPU* vs, CFloatingTypeGPU* DmInvs)
{
	CFloatingTypeGPU* v0 = vs + 3 * tetVIds[0];
	CFloatingTypeGPU* v1 = vs + 3 * tetVIds[1];
	CFloatingTypeGPU* v2 = vs + 3 * tetVIds[2];
	CFloatingTypeGPU* v3 = vs + 3 * tetVIds[3];

	FloatingTypeGPU Ds[9];
	FloatingTypeGPU F[9]; 
	CuMatrix::vec3Minus(v1, v0, Ds);
	CuMatrix::vec3Minus(v2, v0, Ds + 3);
	CuMatrix::vec3Minus(v3, v0, Ds + 6);
	CFloatingTypeGPU* DmInv = DmInvs + tetId * 9;
	//printf("Ds:\n");
	//CuMatrix::printMat3(Ds);

	CuMatrix::mat3MatProduct(Ds, DmInv, F);
	//printf("F:\n");
	//CuMatrix::printMat3(F);

	CFloatingTypeGPU a = 1 + miu / lmbd;

	CFloatingTypeGPU Phi_D = 0.5f * (CuMatrix::mat3FNormSquare(F) - 3.f);
	CFloatingTypeGPU detF = CuMatrix::mat3Determinant(F);
	CFloatingTypeGPU Phi_H = 0.5f * SQR(detF - a);

	FloatingTypeGPU E = A * (miu * Phi_D + lmbd * Phi_H);

	CFloatingTypeGPU restEnergy = 0.5 * A * lmbd * SQR(1.f - a);

	E -= restEnergy;

	return E;
}

GPU_CPU_INLINE_FUNC  void  GAIA::evaluateNeoHookeanMaterialForceAndHessian(CFloatingTypeGPU miu, CFloatingTypeGPU lmbd, CFloatingTypeGPU A, int32_t tetId, int32_t vOrderInTet,
	const int32_t* tetVIds, CFloatingTypeGPU* vs, CFloatingTypeGPU* vsPrev, CFloatingTypeGPU* DmInvs, FloatingTypeGPU* force, FloatingTypeGPU* h,
	CFloatingTypeGPU dampingHydrostatic, CFloatingTypeGPU dampingDeviatoric, CFloatingTypeGPU dt)
{

	CFloatingTypeGPU* v0 = vs + VERTEX_BUFFER_STRIDE * tetVIds[0];
	CFloatingTypeGPU* v1 = vs + VERTEX_BUFFER_STRIDE * tetVIds[1];
	CFloatingTypeGPU* v2 = vs + VERTEX_BUFFER_STRIDE * tetVIds[2];
	CFloatingTypeGPU* v3 = vs + VERTEX_BUFFER_STRIDE * tetVIds[3];

	FloatingTypeGPU Ds[9];
	FloatingTypeGPU F[9];

	CuMatrix::vec3Minus(v1, v0, Ds);
	CuMatrix::vec3Minus(v2, v0, Ds + 3);
	CuMatrix::vec3Minus(v3, v0, Ds + 6);
	CFloatingTypeGPU* DmInv = DmInvs + tetId * 9;
	//printf("Ds:\n");
	//CuMatrix::printMat3(Ds);

	CuMatrix::mat3MatProduct(Ds, DmInv, F);
	//printf("F:\n");
	//CuMatrix::printMat3(F);

	CFloatingTypeGPU a = 1 + miu / lmbd;

	//CFloatingTypeGPU Phi_D = 0.5f * (CuMatrix::mat3FNormSquare(F) - 3.f);
	CFloatingTypeGPU detF = CuMatrix::mat3Determinant(F);
	//CFloatingTypeGPU Phi_H = 0.5f * SQR(detF - a);

	// CFloatingTypeGPU E = A * (miu * Phi_D + lmbd * Phi_H);

	// printf("Phi_D: %f | Phi_H: %f | E: %f\n", Phi_D, detF, Phi_H);

	FloatingTypeGPU* dPhi_D_dF = F;

	// printf("dPhi_D_dF:\n");
	// CuMatrix::printFloatVec(dPhi_D_dF, 9);

	CFloatingTypeGPU F1_1 = F[0];
	CFloatingTypeGPU F2_1 = F[1];
	CFloatingTypeGPU F3_1 = F[2];
	CFloatingTypeGPU F1_2 = F[3 * 1];
	CFloatingTypeGPU F2_2 = F[1 + 3 * 1];
	CFloatingTypeGPU F3_2 = F[2 + 3 * 1];
	CFloatingTypeGPU F1_3 = F[3 * 2];
	CFloatingTypeGPU F2_3 = F[1 + 3 * 2];
	CFloatingTypeGPU F3_3 = F[2 + 3 * 2];

	FloatingTypeGPU ddetF_dF[9] =
	{ F2_2 * F3_3 - F2_3 * F3_2,
	  F1_3 * F3_2 - F1_2 * F3_3,
	  F1_2 * F2_3 - F1_3 * F2_2,
	  F2_3 * F3_1 - F2_1 * F3_3,
	  F1_1 * F3_3 - F1_3 * F3_1,
	  F1_3 * F2_1 - F1_1 * F2_3,
	  F2_1 * F3_2 - F2_2 * F3_1,
	  F1_2 * F3_1 - F1_1 * F3_2,
	  F1_1 * F2_2 - F1_2 * F2_1 };

	// printf("ddetF_dF:\n");
	// CuMatrix::printFloatVec(ddetF_dF, 9);

	CuMatrix::Mat9x9Static<FloatingTypeGPU> d2E_dF_dF;

	CuMatrix::vec9OuterProduct(ddetF_dF, ddetF_dF, d2E_dF_dF);

	CFloatingTypeGPU k = detF - a;
	d2E_dF_dF(0, 4) += k * F3_3;
	d2E_dF_dF(4, 0) += k * F3_3;
	d2E_dF_dF(0, 5) += k * -F2_3;
	d2E_dF_dF(5, 0) += k * -F2_3;
	d2E_dF_dF(0, 7) += k * -F3_2;
	d2E_dF_dF(7, 0) += k * -F3_2;
	d2E_dF_dF(0, 8) += k * F2_2;
	d2E_dF_dF(8, 0) += k * F2_2;

	d2E_dF_dF(1, 3) += k * -F3_3;
	d2E_dF_dF(3, 1) += k * -F3_3;
	d2E_dF_dF(1, 5) += k * F1_3;
	d2E_dF_dF(5, 1) += k * F1_3;
	d2E_dF_dF(1, 6) += k * F3_2;
	d2E_dF_dF(6, 1) += k * F3_2;
	d2E_dF_dF(1, 8) += k * -F1_2;
	d2E_dF_dF(8, 1) += k * -F1_2;

	d2E_dF_dF(2, 3) += k * F2_3;
	d2E_dF_dF(3, 2) += k * F2_3;
	d2E_dF_dF(2, 4) += k * -F1_3;
	d2E_dF_dF(4, 2) += k * -F1_3;
	d2E_dF_dF(2, 6) += k * -F2_2;
	d2E_dF_dF(6, 2) += k * -F2_2;
	d2E_dF_dF(2, 7) += k * F1_2;
	d2E_dF_dF(7, 2) += k * F1_2;

	d2E_dF_dF(3, 7) += k * F3_1;
	d2E_dF_dF(7, 3) += k * F3_1;
	d2E_dF_dF(3, 8) += k * -F2_1;
	d2E_dF_dF(8, 3) += k * -F2_1;

	d2E_dF_dF(4, 6) += k * -F3_1;
	d2E_dF_dF(6, 4) += k * -F3_1;
	d2E_dF_dF(4, 8) += k * F1_1;
	d2E_dF_dF(8, 4) += k * F1_1;

	d2E_dF_dF(5, 6) += k * F2_1;
	d2E_dF_dF(6, 5) += k * F2_1;
	d2E_dF_dF(5, 7) += k * -F1_1;
	d2E_dF_dF(7, 5) += k * -F1_1;

	d2E_dF_dF.multiplyBy(lmbd);

	d2E_dF_dF(0, 0) += miu;
	d2E_dF_dF(1, 1) += miu;
	d2E_dF_dF(2, 2) += miu;
	d2E_dF_dF(3, 3) += miu;
	d2E_dF_dF(4, 4) += miu;
	d2E_dF_dF(5, 5) += miu;
	d2E_dF_dF(6, 6) += miu;
	d2E_dF_dF(7, 7) += miu;
	d2E_dF_dF(8, 8) += miu;

	d2E_dF_dF.multiplyBy(A);

	// printf("d2E_dF_dF:\n");
	// CuMatrix::printMat(d2E_dF_dF.data, 9, 9);

	//std::cout << "d2E_dF_dF:\n" << d2E_dF_dF << std::endl;

	CuMatrix::vec9Mul(dPhi_D_dF, miu, dPhi_D_dF);
	CuMatrix::vec9Mul(ddetF_dF, lmbd * k, ddetF_dF);

	FloatingTypeGPU dE_dF[9];

	CuMatrix::vec9Add(dPhi_D_dF, ddetF_dF, dE_dF);

	CuMatrix::vec9Mul(dE_dF, A, dE_dF);

	/*printf("dE_dF of tet %d:\n", tetId);
	CuMatrix::printFloatVec(dE_dF, 9);*/

	//std::cout << "dE_dF:\n" << dE_dF << std::endl;

	CFloatingTypeGPU DmInv1_1 = DmInv[0];
	CFloatingTypeGPU DmInv2_1 = DmInv[1];
	CFloatingTypeGPU DmInv3_1 = DmInv[2];
	CFloatingTypeGPU DmInv1_2 = DmInv[3 * 1];
	CFloatingTypeGPU DmInv2_2 = DmInv[1 + 3 * 1];
	CFloatingTypeGPU DmInv3_2 = DmInv[2 + 3 * 1];
	CFloatingTypeGPU DmInv1_3 = DmInv[3 * 2];
	CFloatingTypeGPU DmInv2_3 = DmInv[1 + 3 * 2];
	CFloatingTypeGPU DmInv3_3 = DmInv[2 + 3 * 2];

	CFloatingTypeGPU ms[4][3] = {
		{-DmInv1_1 - DmInv2_1 - DmInv3_1, -DmInv1_2 - DmInv2_2 - DmInv3_2,  -DmInv1_3 - DmInv2_3 - DmInv3_3},
		{DmInv1_1, DmInv1_2, DmInv1_3},
		{DmInv2_1, DmInv2_2, DmInv2_3},
		{DmInv3_1, DmInv3_2, DmInv3_3}
	};

	CFloatingTypeGPU m1 = ms[vOrderInTet][0], m2 = ms[vOrderInTet][1], m3 = ms[vOrderInTet][2];
	assembleVertexVForceAndHessian(dE_dF, &d2E_dF_dF, m1, m2, m3, force, h);

	if (dampingHydrostatic > 0.f || dampingDeviatoric > 0.f)
	{
		FloatingTypeGPU dampingH[9];
		CuMatrix::vec9Mul(h, dampingHydrostatic, dampingH);

		FloatingTypeGPU tmp = (m1 * m1 + m2 * m2 + m3 * m3) * miu * A;

		h[0] += tmp;
		h[4] += tmp;
		h[8] += tmp;
		tmp *= dampingDeviatoric;
		dampingH[0] += tmp;
		dampingH[4] += tmp;
		dampingH[8] += tmp;
		CuMatrix::vec9Mul(dampingH, 1 / dt, dampingH);

		CFloatingTypeGPU* v = vs + VERTEX_BUFFER_STRIDE * tetVIds[vOrderInTet];
		CFloatingTypeGPU* vPrev = vsPrev + VERTEX_BUFFER_STRIDE * tetVIds[vOrderInTet];
		FloatingTypeGPU displacement[3];
		CuMatrix::vec3Minus(vPrev, v, displacement);

		FloatingTypeGPU dampingForce[3];
		CuMatrix::mat3VecProduct(dampingH, displacement, dampingForce);
		CuMatrix::vec3Add(force, dampingForce, force);

		CuMatrix::vec9Add(h, dampingH, h);
	}

	//printf("dE_dF of tet %d:\n", tetId);
	//CuMatrix::printFloatVec(dE_dF, 9);

	//printf("force of tet %d:\n-------------\n", tetId);
	//CuMatrix::printFloatVec(force, 3); 

}

GPU_CPU_INLINE_FUNC  void assembleVertexVForceAndHessian(CFloatingTypeGPU* dE_dF, MatType<FloatingTypeGPU>* d2E_dF_dF, CFloatingTypeGPU m1, CFloatingTypeGPU m2, CFloatingTypeGPU m3,
	FloatingTypeGPU* force, FloatingTypeGPU* h)
{
	CFloatingTypeGPU A1 = dE_dF[0];
	CFloatingTypeGPU A2 = dE_dF[1];
	CFloatingTypeGPU A3 = dE_dF[2];
	CFloatingTypeGPU A4 = dE_dF[3];
	CFloatingTypeGPU A5 = dE_dF[4];
	CFloatingTypeGPU A6 = dE_dF[5];
	CFloatingTypeGPU A7 = dE_dF[6];
	CFloatingTypeGPU A8 = dE_dF[7];
	CFloatingTypeGPU A9 = dE_dF[8];

	// force is the negative of gradient
	force[0] -= A1 * m1 + A4 * m2 + A7 * m3;
	force[1] -= A2 * m1 + A5 * m2 + A8 * m3;
	force[2] -= A3 * m1 + A6 * m2 + A9 * m3;

	/*const CuMatrix::Mat9x9Abstract<FloatingTypeGPU>& H = *d2E_dF_dF;
	h[0] += m1 * (H(0, 0) * m1 + H(3, 0) * m2 + H(6, 0) * m3) + m2 * (H(0, 3) * m1 + H(3, 3) * m2 + H(6, 3) * m3) + m3 * (H(0, 6) * m1 + H(3, 6) * m2 + H(6, 6) * m3);
	h[1] += m1 * (H(1, 0) * m1 + H(4, 0) * m2 + H(7, 0) * m3) + m2 * (H(1, 3) * m1 + H(4, 3) * m2 + H(7, 3) * m3) + m3 * (H(1, 6) * m1 + H(4, 6) * m2 + H(7, 6) * m3);
	h[2] += m1 * (H(2, 0) * m1 + H(5, 0) * m2 + H(8, 0) * m3) + m2 * (H(2, 3) * m1 + H(5, 3) * m2 + H(8, 3) * m3) + m3 * (H(2, 6) * m1 + H(5, 6) * m2 + H(8, 6) * m3);
	h[3] += m1 * (H(0, 1) * m1 + H(3, 1) * m2 + H(6, 1) * m3) + m2 * (H(0, 4) * m1 + H(3, 4) * m2 + H(6, 4) * m3) + m3 * (H(0, 7) * m1 + H(3, 7) * m2 + H(6, 7) * m3);
	h[4] += m1 * (H(1, 1) * m1 + H(4, 1) * m2 + H(7, 1) * m3) + m2 * (H(1, 4) * m1 + H(4, 4) * m2 + H(7, 4) * m3) + m3 * (H(1, 7) * m1 + H(4, 7) * m2 + H(7, 7) * m3);
	h[5] += m1 * (H(2, 1) * m1 + H(5, 1) * m2 + H(8, 1) * m3) + m2 * (H(2, 4) * m1 + H(5, 4) * m2 + H(8, 4) * m3) + m3 * (H(2, 7) * m1 + H(5, 7) * m2 + H(8, 7) * m3);
	h[6] += m1 * (H(0, 2) * m1 + H(3, 2) * m2 + H(6, 2) * m3) + m2 * (H(0, 5) * m1 + H(3, 5) * m2 + H(6, 5) * m3) + m3 * (H(0, 8) * m1 + H(3, 8) * m2 + H(6, 8) * m3);
	h[7] += m1 * (H(1, 2) * m1 + H(4, 2) * m2 + H(7, 2) * m3) + m2 * (H(1, 5) * m1 + H(4, 5) * m2 + H(7, 5) * m3) + m3 * (H(1, 8) * m1 + H(4, 8) * m2 + H(7, 8) * m3);
	h[8] += m1 * (H(2, 2) * m1 + H(5, 2) * m2 + H(8, 2) * m3) + m2 * (H(2, 5) * m1 + H(5, 5) * m2 + H(8, 5) * m3) + m3 * (H(2, 8) * m1 + H(5, 8) * m2 + H(8, 8) * m3);*/

	FloatingTypeGPU HL_Row1[9];
	FloatingTypeGPU HL_Row2[9];
	FloatingTypeGPU HL_Row3[9];

	FloatingTypeGPU* HL[3] = { HL_Row1, HL_Row2, HL_Row3 };

	for (int32_t iCol = 0; iCol < 9; iCol++)
	{
		HL_Row1[iCol] = (*d2E_dF_dF)(0, iCol) * m1;
		HL_Row1[iCol] += (*d2E_dF_dF)(3, iCol) * m2;
		HL_Row1[iCol] += (*d2E_dF_dF)(6, iCol) * m3;

		HL_Row2[iCol] = (*d2E_dF_dF)(1, iCol) * m1;
		HL_Row2[iCol] += (*d2E_dF_dF)(4, iCol) * m2;
		HL_Row2[iCol] += (*d2E_dF_dF)(7, iCol) * m3;


		HL_Row3[iCol] = (*d2E_dF_dF)(2, iCol) * m1;
		HL_Row3[iCol] += (*d2E_dF_dF)(5, iCol) * m2;
		HL_Row3[iCol] += (*d2E_dF_dF)(8, iCol) * m3;
	}

	for (int32_t iRow = 0; iRow < 3; iRow++) {
		h[iRow] += HL[iRow][0] * m1 + HL[iRow][3] * m2 + HL[iRow][6] * m3;
		h[iRow + 3] += HL[iRow][1] * m1 + HL[iRow][4] * m2 + HL[iRow][7] * m3;
		h[iRow + 6] += HL[iRow][2] * m1 + HL[iRow][5] * m2 + HL[iRow][8] * m3;
	}

}. for comparison here is working metal version of xpbd stable neo hookean. but in need VBD (with hessian and force base formation). METAL_FUNC void reduceAndSolve(
    uint simdIndex,
    uint vertexIndex,
    float3 force,
    float3x3 hessian,
    constant float3* prevPositions,
    device float3* positions,
    constant float3* preIterPositions,
    constant float3* originalPositions,
    constant float3* inertias,
    StepInfo stepInfo,
    SimulationConfigurationGPU configuration,
    float3 p)
{
    float3 totalForce = simd_sum(force);

    if (simdIndex == 0) {
        float3 currentForce = totalForce;
        
        p = p.x == FLT_MAX ? preIterPositions[vertexIndex].xyz : p;
        positions[vertexIndex].xyz = p + currentForce;
    }
}

kernel void solveXPBDNeoHookean(
    device float3 *positions [[buffer(0)]],
    constant TetConstraintDescriptorGPU *tetDescriptors [[buffer(1)]],
    constant StepInfo &stepInfo [[buffer(2)]],
    constant SimulationConfigurationGPU &configuration [[buffer(3)]],
    constant float3 *prevPositions [[buffer(4)]],
    constant float3 *inertias [[buffer(5)]],
    constant float3 *preIterPositions [[buffer(6)]],
    constant float3 *originalPositions [[buffer(7)]],
    constant uint &descriptorsCount [[buffer(8)]],
    constant float *invMasses[[buffer(9)]],
    uint gid [[thread_position_in_grid]],
    uint simdIndex [[thread_index_in_simdgroup]],
    uint simdgroupsPerThreadgroup [[simdgroups_per_threadgroup]]
) {
    if (gid >= descriptorsCount) { return; }
    
    const TetConstraintDescriptorGPU descriptor = tetDescriptors[gid];
    const uint4 tet = descriptor.indices;
    const uint vertexIndex = tet[descriptor.indexInTet];
    float3 correction = float3(0);
    float3x3 hessian = float3x3(0);
    float3 p = FLT_MAX;

    if (descriptor.indices.w != UINT_MAX) {
        p = preIterPositions[int(vertexIndex)];
        const float dt = stepInfo.dt;
        const uint4 tet = descriptor.indices;
        const float3x3 invRestMat = descriptor.invRestMat;
        const float3x3 invRestMatTr = transpose(invRestMat);
        const uint indexInTet = descriptor.indexInTet;
        const float invRestVolume = descriptor.inverseRestVolume; // actually inversed in this case
        float complianceDev = 0.125; // stretchingCompliance
        float complianceHyd = 0.25; // volumeCompliance
        
        float3 x1 = preIterPositions[tet.x];
        float3 x2 = preIterPositions[tet.y];
        float3 x3 = preIterPositions[tet.z];
        float3 x4 = preIterPositions[tet.w];
        
        float4 w = float4(invMasses[tet.x], invMasses[tet.y], invMasses[tet.z], invMasses[tet.w]);

        float3x3 ds = float3x3(x2 - x1, x3 - x1, x4 - x1);
        float3x3 f = ds * invRestMat;

        float3 f1 = f.columns[0];
        float3 f2 = f.columns[1];
        float3 f3 = f.columns[2];
        float tr = length_squared(f1) + length_squared(f2) + length_squared(f3);

        // Deviatoric term
        float cDev = tr - 3.0;
        float3x3 dcDev = 2.0 * f;

        float3x3 dpDev = dcDev * invRestMatTr;
        float3 gradDev[4] = { 0, dpDev.columns[0], dpDev.columns[1], dpDev.columns[2] };
        gradDev[0] = -gradDev[1] - gradDev[2] - gradDev[3];

        float wDev = length_squared(gradDev[0]) * w.x +
        length_squared(gradDev[1]) * w.y +
        length_squared(gradDev[2]) * w.z +
        length_squared(gradDev[3]) * w.w;

        float alphaDev = complianceDev / (dt * dt) * (invRestVolume > 0 ? invRestVolume : 1.0);
        float deltaDeviatoric = -cDev / (wDev + alphaDev);

        // Hydrostatic term
        float cHyd = determinant(f) - 1.0;
        float3x3 dcHyd = float3x3(cross(f2, f3), cross(f3, f1), cross(f1, f2));

        float3x3 dpHyd = dcHyd * invRestMatTr;
        float3 gradHyd[4] = { 0, dpHyd.columns[0], dpHyd.columns[1], dpHyd.columns[2] };
         gradHyd[0] = -gradHyd[1] - gradHyd[2] - gradHyd[3];

        float wHyd = length_squared(gradHyd[0]) * w.x +
        length_squared(gradHyd[1]) * w.y +
        length_squared(gradHyd[2]) * w.z +
        length_squared(gradHyd[3]) * w.w;

        float alphaHyd = complianceHyd / (dt * dt) * (invRestVolume > 0 ? invRestVolume : 1.0);
        float deltaHydrostatic = -cHyd / (wHyd + alphaHyd);

        if (
            !isnan(deltaHydrostatic) && !isinf(deltaHydrostatic) &&
            !isnan(deltaDeviatoric) && !isinf(deltaDeviatoric) &&
            all(!isnan(gradDev[indexInTet])) &&
            all(!isnan(gradHyd[indexInTet])) &&
            all(!isinf(gradDev[indexInTet])) &&
            all(!isinf(gradHyd[indexInTet]))
            ) {
            correction = (deltaDeviatoric * gradDev[indexInTet] * w[indexInTet] + deltaHydrostatic * gradHyd[indexInTet] * w[indexInTet]) * 0.125;
        }
    }
      
    reduceAndSolve(simdIndex, vertexIndex, correction, hessian,
                   prevPositions, positions, preIterPositions, originalPositions,
                   inertias, stepInfo, configuration, p);
}