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;

    public string name;

    State state = State.unknown;

    ConstraintType constraintType;

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

        Line l1 = null;
        Line l2 = null;
        float length = 0;

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

                l1.setConstraint(this);
                name = l1.name + "=" + length.ToString();
                break;
            case ConstraintType.LineEqual:
                l1 = (Line)obj1;
                l2 = (Line)obj2;

                l1.setConstraint(this);
                l2.setConstraint(this);
                name = l1.name + "=" + l2.name;
                break;
            case ConstraintType.LineNormal:
                l1 = (Line)obj1;
                l2 = (Line)obj2;

                l1.setConstraint(this);
                l2.setConstraint(this);
                name = l1.name + "⊥" + l2.name;
                break;
        }
    }

    public float getConstraintValue()
    {
        float result = 0;

        Line l1 = null;
        Line l2 = null;

        float length = 0;

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

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

        return result;
    }

    public Gradient getGradient()
    {
        Line l1 = null;
        Line l2 = null;

        float length = 0;

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

                return l1.getGradient();
            case ConstraintType.LineEqual:
                l1 = (Line)Object1;
                l2 = (Line)Object2;
                if(l1.lineState > l2.lineState)
                {
                    return l1.getGradient();
                }
                else
                {
                    return l2.getGradient();
                }
            case ConstraintType.LineNormal:
                l1 = (Line)Object1;
                l2 = (Line)Object2;
                if(l1.lineState > l2.lineState)
                {
                    return new Gradient(l2.getUnknownPoint() ,l1.direction);
                }
                else
                {
                    return new Gradient(l1.getUnknownPoint() ,l2.direction);
                }
        }
        return null;
    }

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

    public State getState()
    {
        return state;
    }

    public void checkState()
    {
        Line l1 = null;
        Line l2 = null;

        float length = 0;

        switch (constraintType)
        {
            case ConstraintType.LineLength:
                l1 = (Line)Object1;
                length = (float)Object2;
                
                setState(l1.lineState);
                break;
            case ConstraintType.LineEqual:
            case ConstraintType.LineNormal:
                l1 = (Line)Object1;
                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 normal;
        Vector3 direction;
        Point tmp;

        Line l1 = null;
        Line l2 = null;

        float length = 0;
        float cos = 0;
        float sin = 0;

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

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

                target.setState(State.confirm);
                break;
            case ConstraintType.LineEqual:
                l1 = (Line)Object1;
                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;
            case ConstraintType.LineNormal:
                l1 = (Line)Object1;
                l2 = (Line)Object2;

                if(l1.lineState == State.confirm)
                {
                    normal = l1.direction.normalized;

                    tmp = l2.getAnotherPoint(target);
                    direction = new Vector3(target.x - tmp.x, target.y - tmp.y, target.z - tmp.z);

                    sin = Vector3.Dot(l1.getVector3(), l2.getVector3()) / l1.getLength() / l2.getLength();
                    cos = Mathf.Sqrt(1 - cos * cos);

                    direction = direction * cos + Vector3.Cross(normal, direction) * sin + normal * Vector3.Dot(normal, direction) * (1 - cos);

                    target.setPoint(direction.x - tmp.x, direction.y - tmp.y, direction.z - tmp.z);
                }
                else if(l2.lineState == State.confirm)
                {
                    normal = l2.direction.normalized;

                    tmp = l1.getAnotherPoint(target);
                    direction = new Vector3(target.x - tmp.x, target.y - tmp.y, target.z - tmp.z);

                    sin = Vector3.Dot(l2.getVector3(), l1.getVector3()) / l2.getLength() / l1.getLength();
                    cos = Mathf.Sqrt(1 - cos * cos);

                    direction = direction * cos + Vector3.Cross(normal, direction) * sin + normal * Vector3.Dot(normal, direction) * (1 - cos);

                    target.setPoint(direction.x - tmp.x, direction.y - tmp.y, direction.z - tmp.z);
                }

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

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

        Line l1 = null;
        Line l2 = null;

        float length = 0;

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

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

                target += tmp.calculateGradient(value);

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

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

                target += tmp.calculateGradient(value);

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

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

                target += tmp.calculateGradient(value);

                break;

        }
    }
}