TODO support 3D

class Constraint_AngleXY implements Constraint {
 Vector3 a;
 Vector3 b;
 Vector3 c;
 double stiffness;
 double _angle;
 Constraint_AngleXY(this.a, this.b, this.c, this.stiffness) {
   _angle = _fangle2(b, a, c);
 }

 relax(stepCoef) {
   var angle = _fangle2(b, a, c);
   var diff = angle - _angle;

   if (diff <= - math.PI)
     diff += 2 * math.PI;
   else if (diff >= math.PI)
     diff -= 2 * math.PI;

   diff *= stepCoef * stiffness;

   a = _frotate(a, b, diff);
   c = _frotate(c, b, -diff);
   b = _frotate(b, a, diff);
   b = _frotate(b, c, -diff);
 }

 _fangle(Vector3 v0, Vector3 v1) =>
   math.atan2(v0.x * v1.y - v0.y * v1.x, v0.x * v1.x + v0.y * v1.y);

 _fangle2(Vector3 v0, vLeft, vRight) =>
   _fangle(vLeft - v0, vRight - v0);

 _frotate(Vector3 v0, Vector3 origin, theta) {
   var x = v0.x - origin.x;
   var y = v0.y - origin.y;
   return new Vector3( x * math.cos(theta) - y * math.sin(theta) + origin.x, x* math.sin(theta) + y* math.cos(theta) + origin.y, v0.z);
 }

}