966 lines
34 KiB
C++
966 lines
34 KiB
C++
//-----------------------------------------------------------------------------
|
|
// 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 {
|
|
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 {
|
|
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 {
|
|
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:
|
|
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) {
|
|
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) {
|
|
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) {
|
|
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_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) {
|
|
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_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;
|
|
}
|
|
}
|