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; }