jsketcher/src/cad/gcs/constr/Parallel.java

package cad.gcs.constr;

import cad.gcs.Constraint;
import cad.gcs.Param;
import cad.math.Vector;

public class Parallel implements Constraint {

  public static final int l1p1x = 0;
  public static final int l1p1y = 1;
  public static final int l1p2x = 2;
  public static final int l1p2y = 3;
  public static final int l2p1x = 4;
  public static final int l2p1y = 5;
  public static final int l2p2x = 6;
  public static final int l2p2y = 7;

  private final Param[] params = new Param[8];

  public Parallel(
      Param _l1p1x,
      Param _l1p1y,
      Param _l1p2x,
      Param _l1p2y,
      Param _l2p1x,
      Param _l2p1y,
      Param _l2p2x,
      Param _l2p2y
  ) {
    params[l1p1x] = _l1p1x;
    params[l1p1y] = _l1p1y;
    params[l1p2x] = _l1p2x;
    params[l1p2y] = _l1p2y;
    params[l2p1x] = _l2p1x;
    params[l2p1y] = _l2p1y;
    params[l2p2x] = _l2p2x;
    params[l2p2y] = _l2p2y;
  }

  public void out(Vector p1, Vector p2, Vector p3, Vector p4) {
    p1.x = params[l1p1x].get();
    p1.y = params[l1p1y].get();
    p2.x = params[l1p2x].get();
    p2.y = params[l1p2y].get();
    p3.x = params[l2p1x].get();
    p3.y = params[l2p1y].get();
    p4.x = params[l2p2x].get();
    p4.y = params[l2p2y].get();
  }

  @Override
  public Param[] getParams() {
    return params;
  }

  @Override
  public double error() {
    double dx1 =  (params[l1p2x].get() - params[l1p1x].get());
    double dy1 =  (params[l1p2y].get() - params[l1p1y].get());
    double dx2 = -(params[l2p2y].get() - params[l2p1y].get());
    double dy2 =  (params[l2p2x].get() - params[l2p1x].get());
    //dot product shows how the lines off to be perpendicular
    double off = dx1 * dx2 + dy1 * dy2;
    return off * off;
  }


  //derivative of ((x-a1)*a2 + a3)^2
  public double partDerivative1(double a1, double a2, double a3, double x) {
    return 2*a2*(-a1*a2 + a2*x+a3);
  }

  //derivative of ((a1-x)*a2 + a3)^2
  public double partDerivative2(double a1, double a2, double a3, double x) {
    return -2*a2*(a1*a2 - a2*x+a3);
  }

  @Override
  public void gradient(double[] out) {

    double x1 = params[l1p1x].get();
    double x2 = params[l1p1y].get();
    double x3 = params[l1p2x].get();
    double x4 = params[l1p2y].get();
    double x6 = params[l2p1x].get();
    double x5 = - params[l2p1y].get();
    double x8 = params[l2p2x].get();
    double x7 = - params[l2p2y].get();


    double c1 = x3 - x1;
    double c2 = x7 - x5;
    double c3 = x4 - x2;
    double c4 = x8 - x6;
//
    //f(x) = ( (x3 - x1) * ( x7 - x5) + (x4 - x2) * (x8 - x6) ) ^ 2   =>
    //f(x) = ( (x3 - x1) * (-x8 + x6) + (x4 - x2) * (x7 - x5) ) ^ 2

    out[l1p1x] = partDerivative2(x3, c2, c3 * c4, x1);
    out[l1p1y] = partDerivative2(x4, c4, c1 * c2, x2);

    out[l1p2x] = partDerivative1(x1, c2, c3 * c4, x3);
    out[l1p2y] = partDerivative1(x2, c4, c1 * c2, x4);

    out[l2p1x] = partDerivative2(x7, c1, c3 * c4, x5);
    out[l2p1y] = partDerivative2(x8, c3, c1 * c2, x6);

    out[l2p2x] = partDerivative1(x5, c1, c3 * c4, x7);
    out[l2p2y] = partDerivative1(x6, c3, c1 * c2, x8);
  }

  public double angle() {
    double dx1 = (params[l1p2x].get() - params[l1p1x].get());
    double dy1 = (params[l1p2y].get() - params[l1p1y].get());
    double dx2 = (params[l2p2x].get() - params[l2p1x].get());
    double dy2 = (params[l2p2y].get() - params[l2p1y].get());

    Vector d1 = new Vector(dx1, dy1);
    Vector d2 = new Vector(dx2, dy2);


    return Math.acos(d1.normalize().dot(d2.normalize())) / Math.PI * 180;
  }

  @Override
  public int pSize() {
    return 8;
  }

  public void set(double[] input) {
    params[l1p1x].set(input[l1p1x]);
    params[l1p1y].set(input[l1p1y]);
    params[l1p2x].set(input[l1p2x]);
    params[l1p2y].set(input[l1p2y]);
    params[l2p1x].set(input[l2p1x]);
    params[l2p1y].set(input[l2p1y]);
    params[l2p2x].set(input[l2p2x]);
    params[l2p2y].set(input[l2p2y]);
  }

}