//BENDS
public static void GetBendForce(int a0, int a1, int a2, float[,] positions, float[] energies, float[,] forces, float bendAEq, float bendKEq)
{
int c;
float[] perp = new float[3];
float[] v10 = new float[3];
float[] v12 = new float[3];
float dot = 0f;
float r10;
float r12;
float[] force0 = new float[3];
float[] force2 = new float[3];
Mathematics.VectorFromArray(positions, a1, a0, v10);
Mathematics.VectorFromArray(positions, a1, a2, v12);
r10 = Mathematics.Magnitude3(v10);
r12 = Mathematics.Magnitude3(v12);
Mathematics.Divide3(v10, r10);
Mathematics.Divide3(v12, r12);
Mathematics.EAngle(
Mathematics.UnsignedAngleRad3(v10, v12, dot) - bendAEq,
bendKEq,
energies
);
Mathematics.Cross3(v10, v12, perp);
Mathematics.Cross3(v10, perp, force0);
Mathematics.Cross3(v12, perp, force2);
Mathematics.Multiply3(force0, -energies[1] / r10);
Mathematics.Multiply3(force2, energies[1] / r12);
for (c = 0; c < 3; c++)
{
forces[a0, c] += force0[c];
forces[a1, c] -= force0[c] + force2[c];
forces[a2, c] += force2[c];
}
}