SVD demo

gistfile1.c

#define HERMITE_O_SHARP 0.9f
#define HERMITE_O_CORNER 0.7f

vec3_t dfm_hermite_feature(vec3_t com, DFMHermite *samples, int count) {
    if (count < 2) return com;

    float min_oa = 1e+20f;
    int n0,n1;
    
    for (int i = 0; i < count; ++i) {
        for (int j = i+1; j < count; ++j) {
            float oa = samples[i].n.x*samples[j].n.x
                     + samples[i].n.y*samples[j].n.y
                     + samples[i].n.z*samples[j].n.z;
            if (oa < min_oa) {
                min_oa = oa;
                n0 = i;
                n1 = j;
            }
        }
    }
    
    if (min_oa > HERMITE_O_SHARP)
        return com;

    double a[HERMITE_SAMPLE_COUNT*3];
    double b[HERMITE_SAMPLE_COUNT];
    
    for (int i = 0; i < count; ++i) {
        b[i] = (samples->s.x - com.x)*samples->n.x
             + (samples->s.y - com.y)*samples->n.y
             + (samples->s.z - com.z)*samples->n.z;
        a[i] = samples->n.x;
        a[i+count] = samples->n.y;
        a[i+2*count] = samples->n.z;
        
        samples++;
    }
    
    double *c = svd_solve(count,3,a,b,0.1);
    
    vec3_t result = {{ com.x + (float)c[0], com.y + (float)c[1], com.z + (float)c[2] }};
    r8_free(c);

    return result;
}