Constraint.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public enum ConstraintType
{
    LineLength,
    LineEqual,
    LineNormal,
    LineNormalPlane,
    LineParallel,
    LineParallelPlane,
    LineCross,
    MidPoint,
    PointOnLine,
    PointInPlane,
    AngleEqual,
    PlaneParallelPlane,
    PlaneNormalPlane
}
public class Constraint
{
    public object Object1;
    public object Object2;

    State state = State.unknown;

    ConstraintType constraintType;

    public Constraint(ConstraintType type, object obj1, object obj2)
    {
        Object1 = obj1;
        Object2 = obj2;
        constraintType = type;

        switch (type)
        {
            case ConstraintType.LineLength:
                Line l = (Line)obj1;
                float length = (float)obj2;

                l.setConstraint(this);
                break;
            case ConstraintType.LineEqual:
                Line l1 = (Line)obj1;
                Line l2 = (Line)obj2;

                l1.setConstraint(this);
                l2.setConstraint(this);
                break;
        }
    }

    public float getConstraintValue()
    {
        float result = 0;
        switch (constraintType)
        {
             case ConstraintType.LineLength:
                Line l = (Line)Object1;
                float length = (float)Object2;

                result = l.getLength() - length;
                break;
            case ConstraintType.LineEqual:
                Line l1 = (Line)Object1;
                Line l2 = (Line)Object2;
                
                if (l1.lineState > l2.lineState)
                {
                    result = l1.getLength() - l2.getLength();
                }
                else
                {
                    result = l2.getLength() - l1.getLength();
                }
                break;
        }

        return result;
    }

    public Gradient getGradient()
    {
        switch (constraintType)
        {
            case ConstraintType.LineLength:
                Line l = (Line)Object1;
                float length = (float)Object2;

                return l.getGradient();
            case ConstraintType.LineEqual:
                Line l1 = (Line)Object1;
                Line l2 = (Line)Object2;
                if(l1.lineState > l2.lineState)
                {
                    return l1.getGradient();
                }
                else
                {
                    return l2.getGradient();
                }
        }
        return null;
    }

    public void setState(State s)
    {
        state = s;
    }

    public State getState()
    {
        return state;
    }

    public void checkState()
    {
        switch (constraintType)
        {
            case ConstraintType.LineLength:
                Line l = (Line)Object1;
                float length = (float)Object2;
                
                setState(l.lineState);
                break;
            case ConstraintType.LineEqual:
                Line l1 = (Line)Object1;
                Line l2 = (Line)Object2;
                
                if(l1.lineState == State.confirm && l2.lineState == State.confirm)
                {
                    setState(State.confirm);
                } 
                else if((l1.lineState == State.confirm && l2.lineState == State.halfconfirm) || (l2.lineState == State.confirm && l1.lineState == State.halfconfirm))
                {
                    setState(State.halfconfirm);
                }
                else
                {
                    setState(State.unknown);
                }
                break;
        }
    }

    public void calculateConstraint(Point target)
    {
        Vector3 direction;
        Point tmp;

        switch (constraintType)
        {
            case ConstraintType.LineLength:
                Line l = (Line)Object1;
                float length = (float)Object2;

                tmp = l.getAnotherPoint(target);
                
                direction = l.getDirection(target);
                target.setPoint(tmp.add(direction * length));

                target.setState(State.confirm);
                break;
            case ConstraintType.LineEqual:
                Line l1 = (Line)Object1;
                Line l2 = (Line)Object2;

                if(l1.lineState == State.confirm)
                {
                    tmp = l2.getAnotherPoint(target);

                    direction = l2.getDirection(target);
                    target.setPoint(tmp.add(direction * l1.getLength()));
                }
                else if(l2.lineState == State.confirm)
                {
                    tmp = l1.getAnotherPoint(target);

                    direction = l1.getDirection(target);
                    target.setPoint(tmp.add(direction * l2.getLength()));
                }

                target.setState(State.confirm);
                break;
        }
    }

    public void tryCalculateConstraint(Point target)
    {
        Gradient tmp;
        float value = 0;

        switch (constraintType)
        {
            case ConstraintType.LineLength:
                Line l = (Line)Object1;
                float length = (float)Object2;

                tmp = getGradient();
                value = getConstraintValue();

                target += tmp.calculateGradient(value);

                break;
            case ConstraintType.LineEqual:
                Line l1 = (Line)Object1;
                Line l2 = (Line)Object2;

                tmp = getGradient();
                value = getConstraintValue();

                target += tmp.calculateGradient(value);

                break;
        }
    }
}