msym_error_t partitionEquivalenceSets(int length, msym_element_t *elements[length], msym_element_t *pelements[length], msym_geometry_t g, int *esl, msym_equivalence_set_t **es, msym_thresholds_t *thresholds) {
int ns = 0, gd = geometryDegenerate(g);
double *e = calloc(length,sizeof(double));
double *s = calloc(length,sizeof(double));
int *sp = calloc(length,sizeof(int)); //set partition
int *ss = calloc(length,sizeof(int)); //set size
double (*ev)[3] = calloc(length,sizeof(double[3]));
double (*ep)[3] = calloc(length,sizeof(double[3]));
double (*vec)[3] = calloc(length, sizeof(double[3]));
double *m = calloc(length, sizeof(double));
for(int i = 0;i < length;i++){
vcopy(elements[i]->v, vec[i]);
m[i] = elements[i]->m;
}
for(int i=0; i < length; i++){
for(int j = i+1; j < length;j++){
double w = m[i]*m[j]/(m[i]+m[j]);
double dist;
double v[3];
double proji[3], projj[3];
vnorm2(vec[i],v);
vproj_plane(vec[j], v, proji);
vscale(w, proji, proji);
vadd(proji,ep[i],ep[i]);
vnorm2(vec[j],v);
vproj_plane(vec[i], v, projj);
vscale(w, projj, projj);
vadd(projj,ep[j],ep[j]);
vsub(vec[j],vec[i],v);
dist = vabs(v);
vscale(w/dist,v,v);
vadd(v,ev[i],ev[i]);
vsub(ev[j],v,ev[j]);
double dij = w*dist; //This is sqrt(I) for a diatomic molecule along an axis perpendicular to the bond with O at center of mass.
e[i] += dij;
e[j] += dij;
s[i] += SQR(dij);
s[j] += SQR(dij);
}
vsub(vec[i],ev[i],ev[i]);
}
for(int i = 0; i < length; i++){
double v[3];
double w = m[i]/2.0;
double dist = vabs(elements[i]->v);
double dii = w*dist;
vscale(w,elements[i]->v,v);
vsub(ev[i],v,ev[i]);
// Plane projection can't really differentiate certain types of structures when we add the initial vector,
// but not doing so will result in huge cancellation errors on degenerate point groups,
// also large masses will mess up the eq check when this is 0.
if(gd) vadd(ep[i],v,ep[i]);
e[i] += dii;
s[i] += SQR(dii);
}
for(int i = 0; i < length; i++){
if(e[i] >= 0.0){
sp[i] = i;
for(int j = i+1; j < length;j++){
if(e[j] >= 0.0){
double vabsevi = vabs(ev[i]), vabsevj = vabs(ev[j]), vabsepi = vabs(ep[i]), vabsepj = vabs(ep[j]);
double eep = 0.0, eev = fabs((vabsevi)-(vabsevj))/((vabsevi)+(vabsevj)), ee = fabs((e[i])-(e[j]))/((e[i])+(e[j])), es = fabs((s[i])-(s[j]))/((s[i])+(s[j]));
if(!(vabsepi < thresholds->zero && vabsepj < thresholds->zero)){
eep = fabs((vabsepi)-(vabsepj))/((vabsepi)+(vabsepj));
}
double max = fmax(eev,fmax(eep,fmax(ee, es)));
if(max < thresholds->equivalence && elements[i]->n == elements[j]->n){
e[j] = max > 0.0 ? -max : -1.0;
sp[j] = i;
}
}
}
e[i] = -1.0;
}
}
for(int i = 0; i < length;i++){
int j = sp[i];
ns += (ss[j] == 0);
ss[j]++;
}
msym_equivalence_set_t *eqs = calloc(ns,sizeof(msym_equivalence_set_t));
msym_element_t **lelements = elements;
msym_element_t **pe = pelements;
if(elements == pelements){
lelements = malloc(sizeof(msym_element_t *[length]));
memcpy(lelements, elements, sizeof(msym_element_t *[length]));
}
for(int i = 0, ni = 0; i < length;i++){
if(ss[i] > 0){
int ei = 0;
eqs[ni].elements = pe;
eqs[ni].length = ss[i];
for(int j = 0; j < length;j++){
if(sp[j] == i){
double err = (e[j] > -1.0) ? fabs(e[j]) : 0.0;
eqs[ni].err = fmax(eqs[ni].err,err);
eqs[ni].elements[ei++] = lelements[j];
}
}
pe += ss[i];
ni++;
}
}
if(elements == pelements){
free(lelements);
}
free(m);
free(vec);
free(s);
free(e);
free(sp);
free(ss);
free(ev);
free(ep);
*es = eqs;
*esl = ns;
return MSYM_SUCCESS;
}
void damage() { if (vnorm2(velocity) > DEATH_THRESHOLD2) die(); }
