/// <summary>
///
/// </summary>
/// <param name="particles"></param>
public override sealed void Calculate(IReadOnlyList <IBody> particles)
{
// TODO revise implementation
throw new NotImplementedException();
// TODO
// cache local Rotation3ds for the target angle
// avoids need to explicitly measure angle deviation
Vec3d p0 = particles[_h0].Position;
Vec3d p1 = particles[_h1].Position;
Vec3d p2 = particles[_h2].Position;
Vec3d p3 = particles[_h3].Position;
Vec3d v01 = p1 - p0;
Vec3d v02 = p2 - p0;
Vec3d v03 = p3 - p0;
// get heights
double h0 = Vec3d.Reject(v02, v01).Length;
double h1 = Vec3d.Reject(v03, v01).Length;
double h = 0.5 / (h0 + h1); // inv mean height
// get projection directions (face normals)
Vec3d n0 = Vec3d.Cross(v01, v02);
Vec3d n1 = Vec3d.Cross(v03, v01);
// cache lengths
double m0 = 1.0 / n0.Length;
double m1 = 1.0 / n1.Length;
// angle error
double angle = Math.Acos(SlurMath.Clamp(Vec3d.Dot(n0, n1) * m0 * m1, -1.0, 1.0));
if (Vec3d.Dot(n1, v02) < 0.0)
{
angle *= -1.0; // negate if convex
}
angle += Math.PI;
// projection magnitude & relevant cotangents
double m = (angle - _targetAngle) * h * 0.5;
double c0 = Vec3d.Dot(v02, v01) * m0;
double c1 = Vec3d.Dot(p1 - p2, v01) * m0;
double c2 = Vec3d.Dot(v03, v01) * m1;
double c3 = Vec3d.Dot(p1 - p3, v01) * m1;
// calculate deltas
_h0.Delta = n0 * (m * c1) + n1 * (m * c3);
_h1.Delta = n0 * (m * c0) + n1 * (m * c2);
_h2.Delta = n0 * -(m * (c0 + c1));
_h3.Delta = n1 * -(m * (c2 + c3));
_h0.Weight = _h1.Weight = _h2.Weight = _h3.Weight = Weight;
}