private double[] springPenalty(Vector3d xi, Vector3d xj, double dist) {
double[] F = new double[6];
// collision normal
Vector3d n = xi.minus(xj);
n = n.dot(1 / n.norm());
F[0] = -(k * (n.x()) * (-2 * proximity + dist));
F[1] = -(k * (n.y()) * (-2 * proximity + dist));
F[2] = -(k * (n.z()) * (-2 * proximity + dist));
F[3] = -F[0];
F[4] = -F[1];
F[5] = -F[2];
return F;
}
