/*
* Copyright (c) 2007-2008 Hypertriton, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
* USE OF THIS SOFTWARE EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* These functions implement placement of entities based on their constraints
* with respect to either single entity or a set of two constrained entities.
*
* They are implemented as generic sketch instructions. These instructions are
* usually going to be generated in the degree-of-freedom analysis stage.
*/
#include
#include "sk.h"
#include "sk_placement.h"
/*
* Basic Point-Distance-Point placement. We preserve the original angle of
* the line defined by the two points.
*/
static int
PtFromPtAtDistance(SK_Constraint *ct, void *self, void *other)
{
M_Vector3 p1 = SK_Pos(self);
M_Vector3 p2 = SK_Pos(other);
M_Real theta;
M_VecVecAngle3(p1, p2, &theta, NULL);
SK_Identity(self);
p2.x -= ct->ct_distance*Cos(theta);
p2.y -= ct->ct_distance*Sin(theta);
SK_Translatev(self, &p2);
return (0);
}
/*
* Basic Point-Distance-Line placement. We move the point in a perpendicular
* fashion with respect to its original position.
*/
static int
PtFromLineAtDistance(SK_Constraint *ct, void *self, void *other)
{
SK_Line *L = other;
M_Vector3 pOrig = SK_Pos(self);
M_Vector3 p1 = SK_Pos(L->p1);
M_Vector3 p2 = SK_Pos(L->p2);
M_Vector3 v, vd, s1, s2;
M_Real mag, u, theta;
/*
* Skip unnecessary computations if this is an incidence relation
* between a line and its endpoints.
*/
if (self == L->p1 || self == L->p2) {
return (0);
}
/* Find the closest point on the line. */
vd = M_VecSub3p(&p2, &p1);
mag = M_VecDistance3(p2, p1);
u = ( ((pOrig.x - p1.x)*(p2.x - p1.x)) +
((pOrig.y - p1.y)*(p2.y - p1.y)) ) / (mag*mag);
v = M_VecAdd3(p1, M_VecScale3p(&vd,u));
if (ct->ct_distance == 0.0) {
SK_Identity(self);
SK_Translatev(self, &v);
return (0);
}
M_VecVecAngle3(p1, p2, &theta, NULL);
theta += M_PI/2.0;
s1.x = v.x + ct->ct_distance*Cos(theta);
s1.y = v.y + ct->ct_distance*Sin(theta);
s2.x = v.x - ct->ct_distance*Cos(theta);
s2.y = v.y - ct->ct_distance*Sin(theta);
SK_Identity(self);
if (M_VecDistance3p(&pOrig,&s1) < M_VecDistance3p(&pOrig,&s2)) {
SK_Translatev(self, &s1);
} else {
SK_Translatev(self, &s2);
}
return (0);
}
#if 0
static int
PtFromDistantCircle(SK_Constraint *ct, void *self, void *other)
{
SK_Point *p = self;
SK_Circle *C1 = other;
M_Vector3 p1 = SK_Pos(C1->p);
SK_Identity(p);
SK_Translatev(p, &p1);
SK_Translate2(p, C1->r + ct->ct_distance, 0.0);
return (0);
}
#endif
/*
* Basic Line-Distance-Line placement.
* XXX parallel
*/
static int
LineFromLineAtDistance(SK_Constraint *ct, void *self, void *other)
{
SK_Line *L = self;
SK_Line *L1 = other;
SK_MatrixCopy(L->p1, L1->p1);
SK_MatrixCopy(L->p2, L1->p2);
SK_Translate2(L->p1, ct->ct_distance, 0.0);
SK_Translate2(L->p2, ct->ct_distance, 0.0);
return (0);
}
/*
* Basic Line-Angle-Line placement.
* XXX
*/
static int
LineFromLineAtAngle(SK_Constraint *ct, void *pSelf, void *pFixed)
{
SK_Line *self = pSelf;
SK_Line *fixed = pFixed;
M_Vector3 vShd, vSelf, vFixed;
M_Real len, theta;
SK_Point *p;
theta = ct->ct_angle;
if (self->p1 == fixed->p1 || self->p1 == fixed->p2) {
vShd = SK_Pos(self->p1);
vSelf = SK_Pos(self->p2);
vFixed = (self->p1==fixed->p1) ? SK_Pos(fixed->p2) :
SK_Pos(fixed->p1);
p = self->p2;
// theta = -ct->ct_angle;
} else if (self->p2 == fixed->p1 || self->p2 == fixed->p2) {
vShd = SK_Pos(self->p2);
vSelf = SK_Pos(self->p1);
vFixed = (self->p2==fixed->p1) ? SK_Pos(fixed->p2) :
SK_Pos(fixed->p1);
p = self->p1;
// theta = ct->ct_angle;
} else {
AG_SetError("No shared point between lines");
return (-1);
}
M_VecSub3v(&vSelf, &vShd);
M_VecSub3v(&vFixed, &vShd);
len = M_VecLen3p(&vSelf);
M_VecNorm3v(&vFixed);
(void)M_VecSub3(vFixed, M_VecNorm3(vSelf));
SK_Identity(p);
SK_Translate2(p,
vShd.x + Cos(theta)*vFixed.x*len - Sin(theta)*vFixed.y*len,
vShd.y + Sin(theta)*vFixed.x*len + Cos(theta)*vFixed.y*len);
return (0);
}
/*
* Compute the position of a point relative to two fixed points from
* distance/incidence constraints. This is a system of two quadratic
* equations describing the intersection of two circles:
*
* x^2 + y^2 = c^2
* (x-a)^2 + y^2 = b^2
*
* If there are multiple solutions for a, we pick whichever minimizes
* the displacement from the original point position.
*/
static int
PtFromPtPt(void *self, SK_Constraint *ct1, void *n1, SK_Constraint *ct2,
void *n2)
{
SK *sk = SKNODE(self)->sk;
M_Vector3 pOrig = SK_Pos(self);
M_Vector3 p1 = SK_Pos(n1);
M_Vector3 p2 = SK_Pos(n2);
M_Real d1 = ct1->ct_distance;
M_Real d2 = ct2->ct_distance;
M_Real d12 = M_VecDistance3p(&p1,&p2);
M_Real a, h, b;
M_Vector3 p, s1, s2;
if (ct1->type != SK_DISTANCE || ct2->type != SK_DISTANCE) {
AG_SetError("Expect Distance with %s and %s",
SKNODE(n1)->name, SKNODE(n2)->name);
return (-1);
}
if (d12 > (d1+d2)) {
AG_SetError("%s and %s are too far apart to satisfy "
"constraint: %.02f > (%.02f+%.02f)",
SKNODE(n1)->name, SKNODE(n2)->name, d12, d1, d2);
return (-1);
}
if (d12 < Fabs(d1-d2)) {
AG_SetError("%s and %s are too close to satisfy "
"constraint: %.02f < |%.02f-%.02f|",
SKNODE(n1)->name, SKNODE(n2)->name, d12, d1, d2);
return (-1);
}
a = (d1*d1 - d2*d2 + d12*d12) / (2.0*d12);
h = Sqrt(d1*d1 - a*a);
p = M_VecLERP3p(&p1, &p2, a/d12);
b = h/d12;
s1.x = p.x - b*(p2.y - p1.y);
s1.y = p.y + b*(p2.x - p1.x);
s1.z = 0.0;
s2.x = p.x + b*(p2.y - p1.y);
s2.y = p.y - b*(p2.x - p1.x);
s2.z = 0.0;
SK_Identity(self);
if (M_VecDistance3p(&s1,&s2) == 0.0) {
SK_Translatev(self, &s1);
sk->nSolutions++;
} else {
if (M_VecDistance3p(&pOrig,&s1) < M_VecDistance3p(&pOrig,&s2)) {
SK_Translatev(self, &s1);
} else {
SK_Translatev(self, &s2);
}
sk->nSolutions += 2;
}
return (0);
}
/*
* Place point at distance d1 from a point and distance d2 from a line.
* We solve a system of quadratic equations describing the intersection of a
* line with a circle:
*
* x^2 + (y - d1)^2 = d2
* y = d2
*
* If there are 2 solutions, we pick whichever minimizes displacement from
* the original position.
*
* TODO OFFSET LINE
*/
static int
PtFromPtLine(void *self, SK_Constraint *ct1, void *n1, SK_Constraint *ct2,
void *n2)
{
SK *sk = SKNODE(self)->sk;
M_Vector3 p = SK_Pos(n1);
SK_Line *L = n2;
M_Vector3 p1 = SK_Pos(L->p1);
M_Vector3 p2 = SK_Pos(L->p2);
M_Real a, b, c, det;
M_Vector3 s[2];
int nSolns = 0;
if (ct1->type != SK_DISTANCE || ct2->type != SK_DISTANCE) {
AG_SetError("Expect Distance with %s and %s",
SKNODE(n1)->name, SKNODE(n2)->name);
return (-1);
}
a = (p2.x - p1.x)*(p2.x - p1.x) +
(p2.y - p1.y)*(p2.y - p1.y);
b = 2.0*( (p2.x - p1.x)*(p1.x - p.x) +
(p2.y - p1.y)*(p1.y - p.y) );
c = p.x*p.x + p.y*p.y +
p1.x*p1.x + p1.y*p1.y -
2.0*(p.x*p1.x + p.y*p1.y) -
(ct1->ct_distance*ct1->ct_distance);
det = b*b - 4.0*a*c;
if (det < 0.0) {
AG_SetError("%s,%s are too far apart to satisfy "
"constraint (det=%f)",
SKNODE(n1)->name, SKNODE(n2)->name, det);
return (-1);
} else if (det == 0.0) {
/* TODO! */
AG_SetError("(P,L)->P: Tangent");
return (-1);
} else {
M_Real e = Sqrt(det);
M_Real u1 = (-b + e) / (2.0*a);
M_Real u2 = (-b - e) / (2.0*a);
if ((u1 < 0.0 || u1 > 1.0) &&
(u2 < 0.0 || u2 > 1.0)) {
if ((u1 < 0.0 && u2 < 0.0) ||
(u1 > 1.0 && u2 > 1.0)) {
AG_SetError(
"%s,%s are too far apart to "
"satisfy constraint (%f/%f)",
SKNODE(n1)->name, SKNODE(n2)->name,
u1, u2);
return (-1);
} else {
if (u1 >= 0.0 && u1 <= 1.0) {
s[nSolns++] = M_VecLERP3p(&p1,&p2,u1);
}
if (u2 >= 0.0 && u2 <= 1.0) {
s[nSolns++] = M_VecLERP3p(&p1,&p2,u2);
}
}
} else {
if (u1 >= 0.0 && u1 <= 1.0)
s[nSolns++] = M_VecLERP3p(&p1,&p2,u1);
if (u2 >= 0.0 && u2 <= 1.0)
s[nSolns++] = M_VecLERP3p(&p1,&p2,u2);
}
}
SK_Identity(self);
if (nSolns == 2) {
M_Vector3 pOrig = SK_Pos(self);
if (M_VecDistance3p(&pOrig,&s[0]) <
M_VecDistance3p(&pOrig,&s[1])) {
SK_Translatev(self, &s[0]);
} else {
SK_Translatev(self, &s[1]);
}
} else if (nSolns == 1) {
SK_Translatev(self, &s[0]);
} else {
AG_SetError("(P,L)->P: No solutions");
return (-1);
}
sk->nSolutions += nSolns;
return (0);
}
/*
* Compute the position of a point p from two known lines L1 and L2 at
* angle a at the shared endpoint. This is a system of two linear equations
* describing the intersection of two lines at distances d1 and d2 from L1
* and L2:
*
* n2 = [-sin(a); cos(a)]
* r2 = d1*cos(a) - d2
*
* Where L2 = (n2,r2).
*
* TODO OFFSET LINES
*/
static int
PtFromLineLine(void *self, SK_Constraint *ct1, void *n1, SK_Constraint *ct2,
void *n2)
{
SK *sk = SKNODE(self)->sk;
SK_Line *L1 = n1;
SK_Line *L2 = n2;
M_Vector3 a1 = SK_Pos(L1->p1);
M_Vector3 a2 = SK_Pos(L1->p2);
M_Vector3 b1 = SK_Pos(L2->p1);
M_Vector3 b2 = SK_Pos(L2->p2);
M_Real uaNum, ubNum, uDiv;
/* TODO */
uaNum = (b2.x - b1.x)*(a1.y - b1.y) - (b2.y - b1.y)*(a1.x - b1.x);
ubNum = (a2.x - a1.x)*(a1.y - b1.y) - (a2.y - a1.y)*(a1.x - b1.x);
uDiv = (b2.y - b1.y)*(a2.x - a1.x) - (b2.x - b1.x)*(a2.y - a1.y);
if (uDiv != 0.0) {
M_Real ua = uaNum/uDiv;
M_Real ub = ubNum/uDiv;
if (ua >= 0.0 && ua <= 1.0 &&
ub >= 0.0 && ub <= 1.0) {
SK_TranslateVec(self,
M_VecAdd3(a1, M_VecScale3(M_VecSub3(a2,a1), ua)));
} else {
AG_SetError("L,L->p: No intersection");
return (-1);
}
} else {
/* TODO */
if (uaNum == 0.0 || ubNum == 0.0) {
AG_SetError("L,L->p: Lines are coincident");
return (-1);
} else {
AG_SetError("L,L->p: Lines are parallel");
return (-1);
}
}
sk->nSolutions++;
return (0);
}
/*
* Compute the position of a Line based on Distances with respect to two
* known Points. This case is underconstrained.
*/
static int
LineFromPtPt(void *self, SK_Constraint *ct1, void *n1, SK_Constraint *ct2,
void *n2)
{
/* TODO */
AG_SetError("TODO");
return (-1);
}
/*
* Compute the position of a Line based on Distance from a known Point and
* Angle with respect to a known Line.
*/
static int
LineFromPtLine(void *self, SK_Constraint *ctPoint, void *n1,
SK_Constraint *ctLine, void *n2)
{
if (ctPoint->type != SK_DISTANCE) {
AG_SetError("Expecting Distance constraint with %s",
SKNODE(n1)->name);
return (-1);
}
if (ctLine->type != SK_ANGLE) {
AG_SetError("Expecting Angle constraint with %s",
SKNODE(n2)->name);
return (-1);
}
/* TODO */
AG_SetError("TODO");
return (-1);
}
static int
LineFromLineLine(void *self, SK_Constraint *ct1, void *n1, SK_Constraint *ct2,
void *n2)
{
/* TODO */
AG_SetError("TODO");
return (-1);
}
const SK_ConstraintPairFn skConstraintPairFns[] = {
{ SK_DISTANCE, "Point:*", "Point:*", PtFromPtAtDistance },
{ SK_DISTANCE, "Point:*", "Line:*", PtFromLineAtDistance },
{ SK_DISTANCE, "Line:*", "Line:*", LineFromLineAtDistance },
{ SK_ANGLE, "Line:*", "Line:*", LineFromLineAtAngle },
};
const SK_ConstraintRingFn skConstraintRingFns[] = {
{
"Point:*",
SK_CONSTRAINT_ANY, "Point:*", SK_CONSTRAINT_ANY, "Point:*",
PtFromPtPt
}, {
"Point:*",
SK_CONSTRAINT_ANY, "Point:*", SK_CONSTRAINT_ANY, "Line:*",
PtFromPtLine
}, {
"Point:*",
SK_CONSTRAINT_ANY, "Line:*", SK_CONSTRAINT_ANY, "Line:*",
PtFromLineLine
}, {
"Line:*",
SK_CONSTRAINT_ANY, "Point:*", SK_CONSTRAINT_ANY, "Point:*",
LineFromPtPt
}, {
"Line:*",
SK_CONSTRAINT_ANY, "Point:*", SK_CONSTRAINT_ANY, "Line:*",
LineFromPtLine
}, {
"Line:*",
SK_CONSTRAINT_ANY, "Line:*", SK_CONSTRAINT_ANY, "Line:*",
LineFromLineLine
}
};
const int skConstraintPairFnCount = sizeof(skConstraintPairFns) /
sizeof(skConstraintPairFns[0]);
const int skConstraintRingFnCount = sizeof(skConstraintRingFns) /
sizeof(skConstraintRingFns[0]);