curvature_gradient.h

#include <iostream>
#include <cmath>
#include "Vector3.h"  // Assumes Vector3 class with standard operations

// Function to compute gradients of kappa with respect to pa, pb, pc, pd
void computeGradients(const Vector3& pa, const Vector3& pb, const Vector3& pc, const Vector3& pd,
                      Vector3& grad_pa, Vector3& grad_pb, Vector3& grad_pc, Vector3& grad_pd) {
    // Step 1: Compute edge vectors
    Vector3 eca = pc - pa;        // pc - pa
    Vector3 eda = pd - pa;        // pd - pa
    Vector3 edb = pd - pb;        // pd - pb
    Vector3 ecb = pc - pb;        // pc - pb
    Vector3 pd_pc = pd - pc;      // pd - pc
    double L = pd_pc.norm();      // Length of pd - pc
    Vector3 ecd = pd_pc / L;      // Normalized direction from pc to pd

    // Step 2: Compute unnormalized normals
    Vector3 N1_un = eca.cross(eda);  // Normal of triangle (pa, pc, pd)
    Vector3 N2_un = edb.cross(ecb);  // Normal of triangle (pb, pd, pc)
    double N1_mag = N1_un.norm();    // Magnitude of N1_un
    double N2_mag = N2_un.norm();    // Magnitude of N2_un

    // Check for degenerate cases
    if (N1_mag < 1e-8 || N2_mag < 1e-8) {
        grad_pa = Vector3(0, 0, 0);
        grad_pb = Vector3(0, 0, 0);
        grad_pc = Vector3(0, 0, 0);
        grad_pd = Vector3(0, 0, 0);
        return;
    }

    // Normalize the normals
    Vector3 N1 = N1_un / N1_mag;  // Normalized normal 1
    Vector3 N2 = N2_un / N2_mag;  // Normalized normal 2

    // Step 3: Compute curvature components
    Vector3 N_sum = N1 + N2;             // Sum of normals
    Vector3 dT = N_sum.cross(ecd);       // Tangent direction
    double dT2 = dT.dot(dT) + 1e-8;      // Squared magnitude of dT with regularization
    Vector3 dN = N2 - N1;                // Difference of normals
    double kappa = dN.dot(dT) / dT2;     // Curvature κ

    // Step 4: Compute gradients with respect to N1 and N2
    Vector3 grad_N1_kappa = (-dT * dT2 + dT2 * ecd.cross(dN) - 2 * kappa * dT.cross(ecd)) / (dT2 * dT2);
    Vector3 grad_N2_kappa = (dT * dT2 + dT2 * ecd.cross(dN) - 2 * kappa * dT.cross(ecd)) / (dT2 * dT2);

    // Step 5: Compute gradient with respect to pa
    Vector3 ecd_cross_grad_N1 = ecd.cross(grad_N1_kappa);
    Vector3 N1_dot_grad_N1 = N1 * N1.dot(grad_N1_kappa);
    grad_pa = (L / N1_mag) * (-ecd_cross_grad_N1 + N1_dot_grad_N1.cross(ecd));

    // Step 6: Compute gradient with respect to pb
    Vector3 ecd_cross_grad_N2 = ecd.cross(grad_N2_kappa);
    Vector3 N2_dot_grad_N2 = N2 * N2.dot(grad_N2_kappa);
    grad_pb = (L / N2_mag) * (ecd_cross_grad_N2 + N2_dot_grad_N2.cross(ecd));

    // Step 7: Compute gradient with respect to ecd for pc and pd
    Vector3 grad_ecd_kappa = (dT2 * dN - 2 * kappa * dT).cross(N_sum) / (dT2 * dT2);

    // Step 8: Compute gradient with respect to pc
    Vector3 grad_N1_pc = (eda.cross(grad_N1_kappa) - N1 * N1.dot(eda.cross(grad_N1_kappa))) / N1_mag;
    Vector3 grad_N2_pc = (edb.cross(grad_N2_kappa) + N2 * N2.dot(edb.cross(grad_N2_kappa))) / N2_mag;
    Vector3 grad_ecd_pc = (-grad_ecd_kappa + ecd * ecd.dot(grad_ecd_kappa)) / L;
    grad_pc = grad_N1_pc + grad_N2_pc + grad_ecd_pc;

    // Step 9: Compute gradient with respect to pd
    Vector3 grad_N1_pd = (-eca.cross(grad_N1_kappa) + N1 * N1.dot(eca.cross(grad_N1_kappa))) / N1_mag;
    Vector3 grad_N2_pd = (-ecb.cross(grad_N2_kappa) - N2 * N2.dot(ecb.cross(grad_N2_kappa))) / N2_mag;
    Vector3 grad_ecd_pd = (grad_ecd_kappa - ecd * ecd.dot(grad_ecd_kappa)) / L;
    grad_pd = grad_N1_pd + grad_N2_pd + grad_ecd_pd;
}

// Example usage
int main() {
    // Define test points
    Vector3 pa(0, 0, 0);  // Point a
    Vector3 pb(1, 0, 0);  // Point b
    Vector3 pc(0, 1, 0);  // Point c
    Vector3 pd(0, 0, 1);  // Point d

    // Variables to store gradients
    Vector3 grad_pa, grad_pb, grad_pc, grad_pd;

    // Compute gradients
    computeGradients(pa, pb, pc, pd, grad_pa, grad_pb, grad_pc, grad_pd);

    // Output results
    std::cout << "Gradient w.r.t. pa: (" << grad_pa.x << ", " << grad_pa.y << ", " << grad_pa.z << ")\n";
    std::cout << "Gradient w.r.t. pb: (" << grad_pb.x << ", " << grad_pb.y << ", " << grad_pb.z << ")\n";
    std::cout << "Gradient w.r.t. pc: (" << grad_pc.x << ", " << grad_pc.y << ", " << grad_pc.z << ")\n";
    std::cout << "Gradient w.r.t. pd: (" << grad_pd.x << ", " << grad_pd.y << ", " << grad_pd.z << ")\n";

    return 0;
}