triangle bending constraint

gistfile1.txt

#include <iostream>

// Assume Vector3 is a simple 3D vector class with basic operations
struct Vector3 {
    float x, y, z;
    Vector3(float x_ = 0, float y_ = 0, float z_ = 0) : x(x_), y(y_), z(z_) {}
    
    Vector3 operator+(const Vector3& v) const { return Vector3(x + v.x, y + v.y, z + v.z); }
    Vector3 operator-(const Vector3& v) const { return Vector3(x - v.x, y - v.y, z - v.z); }
    Vector3 operator*(float s) const { return Vector3(x * s, y * s, z * s); }
    
    Vector3 cross(const Vector3& v) const {
        return Vector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
    }
    float dot(const Vector3& v) const { return x * v.x + y * v.y + z * v.z; }
    float squaredNorm() const { return dot(*this); }
};

// Compute the scalar s using qi
float computeS(const Vector3& qb, const Vector3& qc, const Vector3& qd) {
    Vector3 u = qb.cross(qc);
    Vector3 v = qb.cross(qd);
    float denominator = v.squaredNorm();
    if (denominator < 1e-6f) {
        return 0.0f; // Avoid division by zero
    }
    return u.dot(v) / denominator;
}

// Gradient of s with respect to pc (affects qc)
Vector3 gradientS_wrt_pc(const Vector3& qb, const Vector3& qd) {
    Vector3 v = qb.cross(qd);
    float denominator = v.squaredNorm();
    if (denominator < 1e-6f) {
        return Vector3(0, 0, 0);
    }
    float qb_norm_sq = qb.squaredNorm();
    float qb_dot_qd = qb.dot(qd);
    Vector3 numerator = qb_norm_sq * qd - qb_dot_qd * qb;
    return numerator / denominator;
}

// Gradient of s with respect to pd (affects qd)
Vector3 gradientS_wrt_pd(const Vector3& qb, const Vector3& qc, const Vector3& qd) {
    Vector3 u = qb.cross(qc);
    Vector3 v = qb.cross(qd);
    float v_sq = v.squaredNorm();
    if (v_sq < 1e-6f) {
        return Vector3(0, 0, 0);
    }
    float s = computeS(qb, qc, qd);
    float qb_norm_sq = qb.squaredNorm();
    float qb_dot_qc = qb.dot(qc);
    float qb_dot_qd = qb.dot(qd);
    Vector3 term1 = (qb_norm_sq * qc - qb_dot_qc * qb) / v_sq;
    Vector3 term2 = 2 * s * (qb_dot_qd * qb - qb_norm_sq * qd) / v_sq;
    return term1 - term2;
}

// Gradient of s with respect to pb (affects qb)
Vector3 gradientS_wrt_pb(const Vector3& qb, const Vector3& qc, const Vector3& qd) {
    Vector3 u = qb.cross(qc);
    Vector3 v = qb.cross(qd);
    float v_sq = v.squaredNorm();
    if (v_sq < 1e-6f) {
        return Vector3(0, 0, 0);
    }
    float s = computeS(qb, qc, qd);
    Vector3 term1 = (v.cross(qc) + qd.cross(u)) / v_sq;
    Vector3 term2 = 2 * s * qd.cross(v) / v_sq;
    return term1 - term2;
}

// Apply one PBD step to enforce C(P) = s - 1 = 0
void applyPBDStep(Vector3& pa, Vector3& pb, Vector3& pc, Vector3& pd,
                  float w_a, float w_b, float w_c, float w_d) {
    // Compute relative positions once
    Vector3 qb = pb - pa;
    Vector3 qc = pc - pa;
    Vector3 qd = pd - pa;

    // Compute scalar constraint
    float s = computeS(qb, qc, qd);
    float constraint_value = s - 1.0f;
    if (std::abs(constraint_value) < 1e-6f) {
        return; // Constraint already satisfied
    }

    // Compute gradients using qi
    Vector3 grad_pb = gradientS_wrt_pb(qb, qc, qd);
    Vector3 grad_pc = gradientS_wrt_pc(qb, qd);
    Vector3 grad_pd = gradientS_wrt_pd(qb, qc, qd);
    Vector3 grad_pa = -(grad_pb + grad_pc + grad_pd); // Derived from sum of others

    // Compute denominator for lambda (step size)
    float denom = w_a * grad_pa.squaredNorm() +
                  w_b * grad_pb.squaredNorm() +
                  w_c * grad_pc.squaredNorm() +
                  w_d * grad_pd.squaredNorm();
    if (denom < 1e-6f) {
        return; // Avoid division by zero
    }

    float lambda = constraint_value / denom;

    // Compute position corrections
    Vector3 delta_pa = -lambda * w_a * grad_pa;
    Vector3 delta_pb = -lambda * w_b * grad_pb;
    Vector3 delta_pc = -lambda * w_c * grad_pc;
    Vector3 delta_pd = -lambda * w_d * grad_pd;

    // Apply corrections
    pa = pa + delta_pa;
    pb = pb + delta_pb;
    pc = pc + delta_pc;
    pd = pd + delta_pd;
}

// Example usage
int main() {
    Vector3 pa(0, 0, 0);
    Vector3 pb(1, 0, 0);
    Vector3 pc(0, 1, 0);
    Vector3 pd(0, 0, 1);
    float w_a = 1.0f, w_b = 1.0f, w_c = 1.0f, w_d = 1.0f;

    applyPBDStep(pa, pb, pc, pd, w_a, w_b, w_c, w_d);

    std::cout << "Updated positions:\n";
    std::cout << "pa: (" << pa.x << ", " << pa.y << ", " << pa.z << ")\n";
    std::cout << "pb: (" << pb.x << ", " << pb.y << ", " << pb.z << ")\n";
    std::cout << "pc: (" << pc.x << ", " << pc.y << ", " << pc.z << ")\n";
    std::cout << "pd: (" << pd.x << ", " << pd.y << ", " << pd.z << ")\n";

    return 0;
}