src/entity.cpp - master - mastercond/Solvespace-qt

src/entity.cpp
993 строки35 KB
phkahler
Add faces perpendicular and parallel constraints.
12 июн 2022, 03:46
12 июн 2022, 03:463fe6349
//-----------------------------------------------------------------------------
// The implementation of our entities in the symbolic algebra system, methods
// to return a symbolic representation of the entity (line by its endpoints,
// circle by center and radius, etc.).
//
// Copyright 2008-2013 Jonathan Westhues.
//-----------------------------------------------------------------------------
#include "solvespace.h"

const hEntity  EntityBase::FREE_IN_3D = { 0 };
const hEntity  EntityBase::NO_ENTITY = { 0 };

bool EntityBase::HasVector() const {
    switch(type) {
        case Type::LINE_SEGMENT:
        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA:
            return true;

        default:
            return false;
    }
}

ExprVector EntityBase::VectorGetExprsInWorkplane(hEntity wrkpl) const {
    if(IsFace()) {
        return FaceGetNormalExprs();
    }
    switch(type) {
        case Type::LINE_SEGMENT:
            return (SK.GetEntity(point[0])->PointGetExprsInWorkplane(wrkpl)).Minus(
                    SK.GetEntity(point[1])->PointGetExprsInWorkplane(wrkpl));

        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA: {
            ExprVector ev = NormalExprsN();
            if(wrkpl == EntityBase::FREE_IN_3D) {
                return ev;
            }
            // Get the offset and basis vectors for this weird exotic csys.
            EntityBase *w = SK.GetEntity(wrkpl);
            ExprVector wu = w->Normal()->NormalExprsU();
            ExprVector wv = w->Normal()->NormalExprsV();

            // Get our coordinates in three-space, and project them into that
            // coordinate system.
            ExprVector result;
            result.x = ev.Dot(wu);
            result.y = ev.Dot(wv);
            result.z = Expr::From(0.0);
            return result;
        }
        default: ssassert(false, "Unexpected entity type");
    }
}

ExprVector EntityBase::VectorGetExprs() const {
    return VectorGetExprsInWorkplane(EntityBase::FREE_IN_3D);
}

Vector EntityBase::VectorGetNum() const {
    if(IsFace()) {
        return FaceGetNormalNum();
    }
    switch(type) {
        case Type::LINE_SEGMENT:
            return (SK.GetEntity(point[0])->PointGetNum()).Minus(
                    SK.GetEntity(point[1])->PointGetNum());

        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA:
            return NormalN();

        default: ssassert(false, "Unexpected entity type");
    }
}

Vector EntityBase::VectorGetRefPoint() const {
    if(IsFace()) {
        return FaceGetPointNum();
    }
    switch(type) {
        case Type::LINE_SEGMENT:
            return ((SK.GetEntity(point[0])->PointGetNum()).Plus(
                     SK.GetEntity(point[1])->PointGetNum())).ScaledBy(0.5);

        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA:
            return SK.GetEntity(point[0])->PointGetNum();

        default: ssassert(false, "Unexpected entity type");
    }
}

Vector EntityBase::VectorGetStartPoint() const {
    switch(type) {
        case Type::LINE_SEGMENT:
            return SK.GetEntity(point[1])->PointGetNum();

        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA:
            return SK.GetEntity(point[0])->PointGetNum();

        default: ssassert(false, "Unexpected entity type");
    }
}

bool EntityBase::IsCircle() const {
    return (type == Type::CIRCLE) || (type == Type::ARC_OF_CIRCLE);
}

Expr *EntityBase::CircleGetRadiusExpr() const {
    if(type == Type::CIRCLE) {
        return SK.GetEntity(distance)->DistanceGetExpr();
    } else if(type == Type::ARC_OF_CIRCLE) {
        return Constraint::Distance(workplane, point[0], point[1]);
    } else ssassert(false, "Unexpected entity type");
}

double EntityBase::CircleGetRadiusNum() const {
    if(type == Type::CIRCLE) {
        return SK.GetEntity(distance)->DistanceGetNum();
    } else if(type == Type::ARC_OF_CIRCLE) {
        Vector c  = SK.GetEntity(point[0])->PointGetNum();
        Vector pa = SK.GetEntity(point[1])->PointGetNum();
        return (pa.Minus(c)).Magnitude();
    } else ssassert(false, "Unexpected entity type");
}

void EntityBase::ArcGetAngles(double *thetaa, double *thetab, double *dtheta) const {
    ssassert(type == Type::ARC_OF_CIRCLE, "Unexpected entity type");

    Quaternion q = Normal()->NormalGetNum();
    Vector u = q.RotationU(), v = q.RotationV();

    Vector c  = SK.GetEntity(point[0])->PointGetNum();
    Vector pa = SK.GetEntity(point[1])->PointGetNum();
    Vector pb = SK.GetEntity(point[2])->PointGetNum();

    Point2d c2  = c.Project2d(u, v);
    Point2d pa2 = (pa.Project2d(u, v)).Minus(c2);
    Point2d pb2 = (pb.Project2d(u, v)).Minus(c2);

    *thetaa = atan2(pa2.y, pa2.x);
    *thetab = atan2(pb2.y, pb2.x);
    *dtheta = *thetab - *thetaa;
    // If the endpoints are coincident, call it a full arc, not a zero arc;
    // useful concept to have when splitting
    while(*dtheta < 1e-6) *dtheta += 2*PI;
    while(*dtheta > (2*PI)) *dtheta -= 2*PI;
}

Vector EntityBase::CubicGetStartNum() const {
    return SK.GetEntity(point[0])->PointGetNum();
}
Vector EntityBase::CubicGetFinishNum() const {
    return SK.GetEntity(point[3+extraPoints])->PointGetNum();
}
ExprVector EntityBase::CubicGetStartTangentExprs() const {
    ExprVector pon  = SK.GetEntity(point[0])->PointGetExprs(),
               poff = SK.GetEntity(point[1])->PointGetExprs();
    return (pon.Minus(poff));
}
ExprVector EntityBase::CubicGetFinishTangentExprs() const {
    ExprVector pon  = SK.GetEntity(point[3+extraPoints])->PointGetExprs(),
               poff = SK.GetEntity(point[2+extraPoints])->PointGetExprs();
    return (pon.Minus(poff));
}
Vector EntityBase::CubicGetStartTangentNum() const {
    Vector pon  = SK.GetEntity(point[0])->PointGetNum(),
           poff = SK.GetEntity(point[1])->PointGetNum();
    return (pon.Minus(poff));
}
Vector EntityBase::CubicGetFinishTangentNum() const {
    Vector pon  = SK.GetEntity(point[3+extraPoints])->PointGetNum(),
           poff = SK.GetEntity(point[2+extraPoints])->PointGetNum();
    return (pon.Minus(poff));
}

bool EntityBase::IsWorkplane() const {
    return (type == Type::WORKPLANE);
}

ExprVector EntityBase::WorkplaneGetOffsetExprs() const {
    return SK.GetEntity(point[0])->PointGetExprs();
}

Vector EntityBase::WorkplaneGetOffset() const {
    return SK.GetEntity(point[0])->PointGetNum();
}

void EntityBase::WorkplaneGetPlaneExprs(ExprVector *n, Expr **dn) const {
    if(type == Type::WORKPLANE) {
        *n = Normal()->NormalExprsN();

        ExprVector p0 = SK.GetEntity(point[0])->PointGetExprs();
        // The plane is n dot (p - p0) = 0, or
        //              n dot p - n dot p0 = 0
        // so dn = n dot p0
        *dn = p0.Dot(*n);
    } else ssassert(false, "Unexpected entity type");
}

bool EntityBase::IsDistance() const {
    return (type == Type::DISTANCE) ||
           (type == Type::DISTANCE_N_COPY);
}
double EntityBase::DistanceGetNum() const {
    if(type == Type::DISTANCE) {
        return SK.GetParam(param[0])->val;
    } else if(type == Type::DISTANCE_N_COPY) {
        return numDistance;
    } else ssassert(false, "Unexpected entity type");
}
Expr *EntityBase::DistanceGetExpr() const {
    if(type == Type::DISTANCE) {
        return Expr::From(param[0]);
    } else if(type == Type::DISTANCE_N_COPY) {
        return Expr::From(numDistance);
    } else ssassert(false, "Unexpected entity type");
}
void EntityBase::DistanceForceTo(double v) {
    if(type == Type::DISTANCE) {
        (SK.GetParam(param[0]))->val = v;
    } else if(type == Type::DISTANCE_N_COPY) {
        // do nothing, it's locked
    } else ssassert(false, "Unexpected entity type");
}

EntityBase *EntityBase::Normal() const {
    return SK.GetEntity(normal);
}

bool EntityBase::IsPoint() const {
    switch(type) {
        case Type::POINT_IN_3D:
        case Type::POINT_IN_2D:
        case Type::POINT_N_COPY:
        case Type::POINT_N_TRANS:
        case Type::POINT_N_ROT_TRANS:
        case Type::POINT_N_ROT_AA:
        case Type::POINT_N_ROT_AXIS_TRANS:
            return true;

        default:
            return false;
    }
}

bool EntityBase::IsNormal() const {
    switch(type) {
        case Type::NORMAL_IN_3D:
        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
        case Type::NORMAL_N_ROT:
        case Type::NORMAL_N_ROT_AA:
            return true;

        default:           return false;
    }
}

Quaternion EntityBase::NormalGetNum() const {
    Quaternion q;
    switch(type) {
        case Type::NORMAL_IN_3D:
            q = Quaternion::From(param[0], param[1], param[2], param[3]);
            break;

        case Type::NORMAL_IN_2D: {
            EntityBase *wrkpl = SK.GetEntity(workplane);
            EntityBase *norm = SK.GetEntity(wrkpl->normal);
            q = norm->NormalGetNum();
            break;
        }
        case Type::NORMAL_N_COPY:
            q = numNormal;
            break;

        case Type::NORMAL_N_ROT:
            q = Quaternion::From(param[0], param[1], param[2], param[3]);
            q = q.Times(numNormal);
            break;

        case Type::NORMAL_N_ROT_AA: {
            q = GetAxisAngleQuaternion(0);
            q = q.Times(numNormal);
            break;
        }

        default: ssassert(false, "Unexpected entity type");
    }
    return q;
}

void EntityBase::NormalForceTo(Quaternion q) {
    switch(type) {
        case Type::NORMAL_IN_3D:
            SK.GetParam(param[0])->val = q.w;
            SK.GetParam(param[1])->val = q.vx;
            SK.GetParam(param[2])->val = q.vy;
            SK.GetParam(param[3])->val = q.vz;
            break;

        case Type::NORMAL_IN_2D:
        case Type::NORMAL_N_COPY:
            // There's absolutely nothing to do; these are locked.
            break;
        case Type::NORMAL_N_ROT: {
            Quaternion qp = q.Times(numNormal.Inverse());

            SK.GetParam(param[0])->val = qp.w;
            SK.GetParam(param[1])->val = qp.vx;
            SK.GetParam(param[2])->val = qp.vy;
            SK.GetParam(param[3])->val = qp.vz;
            break;
        }

        case Type::NORMAL_N_ROT_AA:
            // Not sure if I'll bother implementing this one
            break;

        default: ssassert(false, "Unexpected entity type");
    }
}

Vector EntityBase::NormalU() const {
    return NormalGetNum().RotationU();
}
Vector EntityBase::NormalV() const {
    return NormalGetNum().RotationV();
}
Vector EntityBase::NormalN() const {
    return NormalGetNum().RotationN();
}

ExprVector EntityBase::NormalExprsU() const {
    return NormalGetExprs().RotationU();
}
ExprVector EntityBase::NormalExprsV() const {
    return NormalGetExprs().RotationV();
}
ExprVector EntityBase::NormalExprsN() const {
    return NormalGetExprs().RotationN();
}

ExprQuaternion EntityBase::NormalGetExprs() const {
    ExprQuaternion q;
    switch(type) {
        case Type::NORMAL_IN_3D:
            q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);
            break;

        case Type::NORMAL_IN_2D: {
            EntityBase *wrkpl = SK.GetEntity(workplane);
            EntityBase *norm = SK.GetEntity(wrkpl->normal);
            q = norm->NormalGetExprs();
            break;
        }
        case Type::NORMAL_N_COPY:
            q = ExprQuaternion::From(numNormal);
            break;

        case Type::NORMAL_N_ROT: {
            ExprQuaternion orig = ExprQuaternion::From(numNormal);
            q = ExprQuaternion::From(param[0], param[1], param[2], param[3]);

            q = q.Times(orig);
            break;
        }

        case Type::NORMAL_N_ROT_AA: {
            ExprQuaternion orig = ExprQuaternion::From(numNormal);
            q = GetAxisAngleQuaternionExprs(0);
            q = q.Times(orig);
            break;
        }

        default: ssassert(false, "Unexpected entity type");
    }
    return q;
}

void EntityBase::PointForceParamTo(Vector p) {
    switch(type) {
        case Type::POINT_IN_3D:
            SK.GetParam(param[0])->val = p.x;
            SK.GetParam(param[1])->val = p.y;
            SK.GetParam(param[2])->val = p.z;
            break;

        case Type::POINT_IN_2D:
            SK.GetParam(param[0])->val = p.x;
            SK.GetParam(param[1])->val = p.y;
            break;

        default: ssassert(false, "Unexpected entity type");
    }
}

void EntityBase::PointForceTo(Vector p) {
    switch(type) {
        case Type::POINT_IN_3D:
            SK.GetParam(param[0])->val = p.x;
            SK.GetParam(param[1])->val = p.y;
            SK.GetParam(param[2])->val = p.z;
            break;

        case Type::POINT_IN_2D: {
            EntityBase *c = SK.GetEntity(workplane);
            p = p.Minus(c->WorkplaneGetOffset());
            SK.GetParam(param[0])->val = p.Dot(c->Normal()->NormalU());
            SK.GetParam(param[1])->val = p.Dot(c->Normal()->NormalV());
            break;
        }

        case Type::POINT_N_TRANS: {
            if(timesApplied == 0) break;
            Vector trans = (p.Minus(numPoint)).ScaledBy(1.0/timesApplied);
            SK.GetParam(param[0])->val = trans.x;
            SK.GetParam(param[1])->val = trans.y;
            SK.GetParam(param[2])->val = trans.z;
            break;
        }

        case Type::POINT_N_ROT_TRANS: {
            // Force only the translation; leave the rotation unchanged. But
            // remember that we're working with respect to the rotated
            // point.
            Vector trans = p.Minus(PointGetQuaternion().Rotate(numPoint));
            SK.GetParam(param[0])->val = trans.x;
            SK.GetParam(param[1])->val = trans.y;
            SK.GetParam(param[2])->val = trans.z;
            break;
        }

        case Type::POINT_N_ROT_AA: {
            // Force only the angle; the axis and center of rotation stay
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Vector normal = Vector::From(param[4], param[5], param[6]);
            Vector u = normal.Normal(0), v = normal.Normal(1);
            Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
            double thetap = atan2(v.Dot(po), u.Dot(po));
            double thetan = atan2(v.Dot(numo), u.Dot(numo));
            double thetaf = (thetap - thetan);
            double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
            double dtheta = thetaf - thetai;
            // Take the smallest possible change in the actual step angle,
            // in order to avoid jumps when you cross from +pi to -pi
            while(dtheta < -PI) dtheta += 2*PI;
            while(dtheta > PI) dtheta -= 2*PI;
            // this extra *2 explains the mystery *4
            SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
            break;
        }

        case Type::POINT_N_ROT_AXIS_TRANS: {
            if(timesApplied == 0) break;
            // is the point on the rotation axis?
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Vector normal = Vector::From(param[4], param[5], param[6]).WithMagnitude(1.0);
            Vector check = numPoint.Minus(offset).Cross(normal);
            if (check.Dot(check) < LENGTH_EPS) { // if so, do extrusion style drag
                Vector trans = (p.Minus(numPoint));
                SK.GetParam(param[7])->val = trans.Dot(normal)/timesApplied;
            } else { // otherwise do rotation style
                Vector u = normal.Normal(0), v = normal.Normal(1);
                Vector po = p.Minus(offset), numo = numPoint.Minus(offset);
                double thetap = atan2(v.Dot(po), u.Dot(po));
                double thetan = atan2(v.Dot(numo), u.Dot(numo));
                double thetaf = (thetap - thetan);
                double thetai = (SK.GetParam(param[3])->val)*timesApplied*2;
                double dtheta = thetaf - thetai;
                // Take the smallest possible change in the actual step angle,
                // in order to avoid jumps when you cross from +pi to -pi
                while(dtheta < -PI) dtheta += 2*PI;
                while(dtheta > PI) dtheta -= 2*PI;
                // this extra *2 explains the mystery *4
                SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2);
            }
            break;
        }

        case Type::POINT_N_COPY:
            // Nothing to do; it's a static copy
            break;

        default: ssassert(false, "Unexpected entity type");
    }
}

Vector EntityBase::PointGetNum() const {
    Vector p;
    switch(type) {
        case Type::POINT_IN_3D:
            p = Vector::From(param[0], param[1], param[2]);
            break;

        case Type::POINT_IN_2D: {
            EntityBase *c = SK.GetEntity(workplane);
            Vector u = c->Normal()->NormalU();
            Vector v = c->Normal()->NormalV();
            p =        u.ScaledBy(SK.GetParam(param[0])->val);
            p = p.Plus(v.ScaledBy(SK.GetParam(param[1])->val));
            p = p.Plus(c->WorkplaneGetOffset());
            break;
        }

        case Type::POINT_N_TRANS: {
            Vector trans = Vector::From(param[0], param[1], param[2]);
            p = numPoint.Plus(trans.ScaledBy(timesApplied));
            break;
        }

        case Type::POINT_N_ROT_TRANS: {
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Quaternion q = PointGetQuaternion();
            p = q.Rotate(numPoint);
            p = p.Plus(offset);
            break;
        }

        case Type::POINT_N_ROT_AA: {
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Quaternion q = PointGetQuaternion();
            p = numPoint.Minus(offset);
            p = q.Rotate(p);
            p = p.Plus(offset);
            break;
        }

        case Type::POINT_N_ROT_AXIS_TRANS: {
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Vector displace = Vector::From(param[4], param[5], param[6])
               .WithMagnitude(SK.GetParam(param[7])->val).ScaledBy(timesApplied);
            Quaternion q = PointGetQuaternion();
            p = numPoint.Minus(offset);
            p = q.Rotate(p);
            p = p.Plus(offset).Plus(displace);
            break;
        }

        case Type::POINT_N_COPY:
            p = numPoint;
            break;

        default: ssassert(false, "Unexpected entity type");
    }
    return p;
}

ExprVector EntityBase::PointGetExprs() const {
    ExprVector r;
    switch(type) {
        case Type::POINT_IN_3D:
            r = ExprVector::From(param[0], param[1], param[2]);
            break;

        case Type::POINT_IN_2D: {
            EntityBase *c = SK.GetEntity(workplane);
            ExprVector u = c->Normal()->NormalExprsU();
            ExprVector v = c->Normal()->NormalExprsV();
            r = c->WorkplaneGetOffsetExprs();
            r = r.Plus(u.ScaledBy(Expr::From(param[0])));
            r = r.Plus(v.ScaledBy(Expr::From(param[1])));
            break;
        }
        case Type::POINT_N_TRANS: {
            ExprVector orig = ExprVector::From(numPoint);
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
            r = orig.Plus(trans.ScaledBy(Expr::From(timesApplied)));
            break;
        }
        case Type::POINT_N_ROT_TRANS: {
            ExprVector orig = ExprVector::From(numPoint);
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
            ExprQuaternion q =
                ExprQuaternion::From(param[3], param[4], param[5], param[6]);
            orig = q.Rotate(orig);
            r = orig.Plus(trans);
            break;
        }
        case Type::POINT_N_ROT_AA: {
            ExprVector orig = ExprVector::From(numPoint);
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
            orig = orig.Minus(trans);
            orig = q.Rotate(orig);
            r = orig.Plus(trans);
            break;
        }
        case Type::POINT_N_ROT_AXIS_TRANS: {
            ExprVector orig = ExprVector::From(numPoint);
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
            ExprVector displace = ExprVector::From(param[4], param[5], param[6])
               .WithMagnitude(Expr::From(1.0)).ScaledBy(Expr::From(timesApplied)).ScaledBy(Expr::From(param[7]));

            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
            orig = orig.Minus(trans);
            orig = q.Rotate(orig);
            r = orig.Plus(trans).Plus(displace);
            break;
        }
        case Type::POINT_N_COPY:
            r = ExprVector::From(numPoint);
            break;

        default: ssassert(false, "Unexpected entity type");
    }
    return r;
}

void EntityBase::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) const {
    if(type == Type::POINT_IN_2D && workplane == wrkpl) {
        // They want our coordinates in the form that we've written them,
        // very nice.
        *u = Expr::From(param[0]);
        *v = Expr::From(param[1]);
    } else {
        // Get the offset and basis vectors for this weird exotic csys.
        EntityBase *w = SK.GetEntity(wrkpl);
        ExprVector wp = w->WorkplaneGetOffsetExprs();
        ExprVector wu = w->Normal()->NormalExprsU();
        ExprVector wv = w->Normal()->NormalExprsV();

        // Get our coordinates in three-space, and project them into that
        // coordinate system.
        ExprVector ev = PointGetExprs();
        ev = ev.Minus(wp);
        *u = ev.Dot(wu);
        *v = ev.Dot(wv);
    }
}

ExprVector EntityBase::PointGetExprsInWorkplane(hEntity wrkpl) const {
    if(wrkpl == Entity::FREE_IN_3D) {
        return PointGetExprs();
    }

    ExprVector r;
    PointGetExprsInWorkplane(wrkpl, &r.x, &r.y);
    r.z = Expr::From(0.0);
    return r;
}

void EntityBase::PointForceQuaternionTo(Quaternion q) {
    ssassert(type == Type::POINT_N_ROT_TRANS, "Unexpected entity type");

    SK.GetParam(param[3])->val = q.w;
    SK.GetParam(param[4])->val = q.vx;
    SK.GetParam(param[5])->val = q.vy;
    SK.GetParam(param[6])->val = q.vz;
}

Quaternion EntityBase::GetAxisAngleQuaternion(int param0) const {
    Quaternion q;
    double theta = timesApplied*SK.GetParam(param[param0+0])->val;
    double s = sin(theta), c = cos(theta);
    q.w = c;
    q.vx = s*SK.GetParam(param[param0+1])->val;
    q.vy = s*SK.GetParam(param[param0+2])->val;
    q.vz = s*SK.GetParam(param[param0+3])->val;
    return q;
}

ExprQuaternion EntityBase::GetAxisAngleQuaternionExprs(int param0) const {
    ExprQuaternion q;

    Expr *theta = Expr::From(timesApplied)->Times(
                  Expr::From(param[param0+0]));
    Expr *c = theta->Cos(), *s = theta->Sin();
    q.w = c;
    q.vx = s->Times(Expr::From(param[param0+1]));
    q.vy = s->Times(Expr::From(param[param0+2]));
    q.vz = s->Times(Expr::From(param[param0+3]));
    return q;
}

Quaternion EntityBase::PointGetQuaternion() const {
    Quaternion q;

    if(type == Type::POINT_N_ROT_AA || type == Type::POINT_N_ROT_AXIS_TRANS) {
        q = GetAxisAngleQuaternion(3);
    } else if(type == Type::POINT_N_ROT_TRANS) {
        q = Quaternion::From(param[3], param[4], param[5], param[6]);
    } else ssassert(false, "Unexpected entity type");

    return q;
}

bool EntityBase::IsFace() const {
    switch(type) {
        case Type::FACE_NORMAL_PT:
        case Type::FACE_XPROD:
        case Type::FACE_N_ROT_TRANS:
        case Type::FACE_N_TRANS:
        case Type::FACE_N_ROT_AA:
        case Type::FACE_ROT_NORMAL_PT:
        case Type::FACE_N_ROT_AXIS_TRANS:
            return true;
        default:
            return false;
    }
}

ExprVector EntityBase::FaceGetNormalExprs() const {
    ExprVector r;
    if(type == Type::FACE_NORMAL_PT) {
        Vector v = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        r = ExprVector::From(v.WithMagnitude(1));
    } else if(type == Type::FACE_XPROD) {
        ExprVector vc = ExprVector::From(param[0], param[1], param[2]);
        ExprVector vn =
            ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        r = vc.Cross(vn);
        r = r.WithMagnitude(Expr::From(1.0));
    } else if(type == Type::FACE_N_ROT_TRANS) {
        // The numerical normal vector gets the rotation; the numerical
        // normal has magnitude one, and the rotation doesn't change that,
        // so there's no need to fix it up.
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        ExprQuaternion q =
            ExprQuaternion::From(param[3], param[4], param[5], param[6]);
        r = q.Rotate(r);
    } else if(type == Type::FACE_N_TRANS) {
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
    } else if((type == Type::FACE_N_ROT_AA) || (type == Type::FACE_ROT_NORMAL_PT)) {
        r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
        r = q.Rotate(r);
    } else ssassert(false, "Unexpected entity type");
    return r;
}

Vector EntityBase::FaceGetNormalNum() const {
    Vector r;
    if(type == Type::FACE_NORMAL_PT) {
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
    } else if(type == Type::FACE_XPROD) {
        Vector vc = Vector::From(param[0], param[1], param[2]);
        Vector vn = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        r = vc.Cross(vn);
    } else if(type == Type::FACE_N_ROT_TRANS) {
        // The numerical normal vector gets the rotation
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
        r = q.Rotate(r);
    } else if(type == Type::FACE_N_TRANS) {
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
    } else if((type == Type::FACE_N_ROT_AA) || (type == Type::FACE_ROT_NORMAL_PT)) {
        r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz);
        Quaternion q = GetAxisAngleQuaternion(3);
        r = q.Rotate(r);
    } else ssassert(false, "Unexpected entity type");
    return r.WithMagnitude(1);
}

ExprVector EntityBase::FaceGetPointExprs() const {
    ExprVector r;
    if((type == Type::FACE_NORMAL_PT) || (type==Type::FACE_ROT_NORMAL_PT)) {
        r = SK.GetEntity(point[0])->PointGetExprs();
    } else if(type == Type::FACE_XPROD) {
        r = ExprVector::From(numPoint);
    } else if(type == Type::FACE_N_ROT_TRANS) {
        // The numerical point gets the rotation and translation.
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
        ExprQuaternion q =
            ExprQuaternion::From(param[3], param[4], param[5], param[6]);
        r = ExprVector::From(numPoint);
        r = q.Rotate(r);
        r = r.Plus(trans);
    } else if(type == Type::FACE_N_ROT_AXIS_TRANS) {
            ExprVector orig = ExprVector::From(numPoint);
            ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
            ExprVector displace = ExprVector::From(param[4], param[5], param[6])
               .WithMagnitude(Expr::From(param[7])).ScaledBy(Expr::From(timesApplied));
            ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
            orig = orig.Minus(trans);
            orig = q.Rotate(orig);
            r = orig.Plus(trans).Plus(displace);
    } else if(type == Type::FACE_N_TRANS) {
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
        r = ExprVector::From(numPoint);
        r = r.Plus(trans.ScaledBy(Expr::From(timesApplied)));
    } else if(type == Type::FACE_N_ROT_AA) {
        ExprVector trans = ExprVector::From(param[0], param[1], param[2]);
        ExprQuaternion q = GetAxisAngleQuaternionExprs(3);
        r = ExprVector::From(numPoint);
        r = r.Minus(trans);
        r = q.Rotate(r);
        r = r.Plus(trans);
    } else ssassert(false, "Unexpected entity type");
    return r;
}

Vector EntityBase::FaceGetPointNum() const {
    Vector r;
    if((type == Type::FACE_NORMAL_PT) || (type==Type::FACE_ROT_NORMAL_PT)) {
        r = SK.GetEntity(point[0])->PointGetNum();
    } else if(type == Type::FACE_XPROD) {
        r = numPoint;
    } else if(type == Type::FACE_N_ROT_TRANS) {
        // The numerical point gets the rotation and translation.
        Vector trans = Vector::From(param[0], param[1], param[2]);
        Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]);
        r = q.Rotate(numPoint);
        r = r.Plus(trans);
    } else if(type == Type::FACE_N_ROT_AXIS_TRANS) {
            Vector offset = Vector::From(param[0], param[1], param[2]);
            Vector displace = Vector::From(param[4], param[5], param[6])
               .WithMagnitude(SK.GetParam(param[7])->val).ScaledBy(timesApplied);
            Quaternion q = PointGetQuaternion();
            r = numPoint.Minus(offset);
            r = q.Rotate(r);
            r = r.Plus(offset).Plus(displace);
    } else if(type == Type::FACE_N_TRANS) {
        Vector trans = Vector::From(param[0], param[1], param[2]);
        r = numPoint.Plus(trans.ScaledBy(timesApplied));
    } else if(type == Type::FACE_N_ROT_AA) {
        Vector trans = Vector::From(param[0], param[1], param[2]);
        Quaternion q = GetAxisAngleQuaternion(3);
        r = numPoint.Minus(trans);
        r = q.Rotate(r);
        r = r.Plus(trans);
    } else ssassert(false, "Unexpected entity type");
    return r;
}

bool EntityBase::HasEndpoints() const {
    return (type == Type::LINE_SEGMENT) ||
           (type == Type::CUBIC) ||
           (type == Type::ARC_OF_CIRCLE);
}
Vector EntityBase::EndpointStart() const {
    if(type == Type::LINE_SEGMENT) {
        return SK.GetEntity(point[0])->PointGetNum();
    } else if(type == Type::CUBIC) {
        return CubicGetStartNum();
    } else if(type == Type::ARC_OF_CIRCLE) {
        return SK.GetEntity(point[1])->PointGetNum();
    } else ssassert(false, "Unexpected entity type");
}
Vector EntityBase::EndpointFinish() const {
    if(type == Type::LINE_SEGMENT) {
        return SK.GetEntity(point[1])->PointGetNum();
    } else if(type == Type::CUBIC) {
        return CubicGetFinishNum();
    } else if(type == Type::ARC_OF_CIRCLE) {
        return SK.GetEntity(point[2])->PointGetNum();
    } else ssassert(false, "Unexpected entity type");
}
static bool PointInPlane(hEntity h, Vector norm, double distance) {
    Vector p = SK.GetEntity(h)->PointGetNum();
    return (fabs(norm.Dot(p) - distance) < LENGTH_EPS);
}
bool EntityBase::IsInPlane(Vector norm, double distance) const {
    switch(type) {
        case Type::LINE_SEGMENT: {
            return PointInPlane(point[0], norm, distance)
                && PointInPlane(point[1], norm, distance);
        }
        case Type::CUBIC:
        case Type::CUBIC_PERIODIC: {
            bool periodic = type == Type::CUBIC_PERIODIC;
            int n = periodic ? 3 + extraPoints : extraPoints;
            int i;
            for (i=0; i<n; i++) {
                if (!PointInPlane(point[i], norm, distance)) return false;
            }
            return true;
        }

        case Type::CIRCLE:
        case Type::ARC_OF_CIRCLE: {
            // If it is an (arc of) a circle, check whether the normals
            // are parallel and the mid point is in the plane.
            Vector n = Normal()->NormalN();
            if (!norm.Equals(n) && !norm.Equals(n.Negated())) return false;
            return PointInPlane(point[0], norm, distance);
        }

        case Type::TTF_TEXT: {
            Vector n = Normal()->NormalN();
            if (!norm.Equals(n) && !norm.Equals(n.Negated())) return false;
            return PointInPlane(point[0], norm, distance)
                && PointInPlane(point[1], norm, distance);
        }

        default:
            return false;
    }
}

void EntityBase::RectGetPointsExprs(ExprVector *eb, ExprVector *ec) const {
    ssassert(type == Type::TTF_TEXT || type == Type::IMAGE,
             "Unexpected entity type");

    EntityBase *a = SK.GetEntity(point[0]);
    EntityBase *o = SK.GetEntity(point[1]);

    // Write equations for each point in the current workplane.
    // This reduces the complexity of resulting equations.
    ExprVector ea = a->PointGetExprsInWorkplane(workplane);
    ExprVector eo = o->PointGetExprsInWorkplane(workplane);

    // Take perpendicular vector and scale it by aspect ratio.
    ExprVector eu = ea.Minus(eo);
    ExprVector ev = ExprVector::From(eu.y, eu.x->Negate(), eu.z).ScaledBy(Expr::From(aspectRatio));

    *eb = eo.Plus(ev);
    *ec = eo.Plus(eu).Plus(ev);
}

void EntityBase::AddEq(IdList<Equation,hEquation> *l, Expr *expr, int index) const {
    Equation eq;
    eq.e = expr;
    eq.h = h.equation(index);
    l->Add(&eq);
}

void EntityBase::GenerateEquations(IdList<Equation,hEquation> *l) const {
    switch(type) {
        case Type::NORMAL_IN_3D: {
            ExprQuaternion q = NormalGetExprs();
            AddEq(l, (q.Magnitude())->Minus(Expr::From(1)), 0);
            break;
        }

        case Type::ARC_OF_CIRCLE: {
            // If this is a copied entity, with its point already fixed
            // with respect to each other, then we don't want to generate
            // the distance constraint!
            if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break;

            // If the two endpoints of the arc are constrained coincident
            // (to make a complete circle), then our distance constraint
            // would be redundant and therefore overconstrain things.
            auto it = std::find_if(SK.constraint.begin(), SK.constraint.end(),
                                   [&](ConstraintBase const &con) {
                                       return (con.group == group) &&
                                              (con.type == Constraint::Type::POINTS_COINCIDENT) &&
                                              ((con.ptA == point[1] && con.ptB == point[2]) ||
                                               (con.ptA == point[2] && con.ptB == point[1]));
                                   });
            if(it != SK.constraint.end()) {
                break;
            }

            Expr *ra = Constraint::Distance(workplane, point[0], point[1]);
            Expr *rb = Constraint::Distance(workplane, point[0], point[2]);
            AddEq(l, ra->Minus(rb), 0);
            break;
        }

        case Type::IMAGE:
        case Type::TTF_TEXT: {
            if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break;
            EntityBase *b = SK.GetEntity(point[2]);
            EntityBase *c = SK.GetEntity(point[3]);
            ExprVector eb = b->PointGetExprsInWorkplane(workplane);
            ExprVector ec = c->PointGetExprsInWorkplane(workplane);

            ExprVector ebp, ecp;
            RectGetPointsExprs(&ebp, &ecp);

            ExprVector beq = eb.Minus(ebp);
            AddEq(l, beq.x, 0);
            AddEq(l, beq.y, 1);
            ExprVector ceq = ec.Minus(ecp);
            AddEq(l, ceq.x, 2);
            AddEq(l, ceq.y, 3);
            break;
        }

        default: // Most entities do not generate equations.
            break;
    }
}