inline bool JunctionArea::sequencesPullBack() {
std::vector<EnteringSequence>::iterator a;
double alongLinePosition, distanceFromLine;
unsigned int i, iLink, tail;
TPointD P;
for (a = m_enteringSequences.begin(); a != m_enteringSequences.end(); ++a) {
i = a->m_head;
iLink = a->m_headLink;
// NOTE: updated tails are stored this way, *DONT* look in a->m_tail
// because these typically store old infos
tail =
currJSGraph->getNode(a->m_initialJoint).getLink(a->m_outerLink)->m_tail;
P = planeProjection(*a->m_graphHolder->getNode(a->m_head));
while (i != tail) {
alongLinePosition = a->m_direction * (m_newJointPosition - P);
distanceFromLine = tdistance(m_newJointPosition, a->m_direction, P);
if (alongLinePosition >= 0 &&
(distanceFromLine / alongLinePosition) <= 0.5)
goto found_pull_back;
// We then take the next arc and check it
if (!a->m_graphHolder->getNode(i).getLink(iLink)->hasAttribute(
SkeletonArc::ROAD))
return 0; // Pull back failed
a->next(i, iLink);
P = planeProjection(*a->m_graphHolder->getNode(i));
if (tdistance(P, a->m_direction, m_newJointPosition) >
std::max(pullBackMul * a->m_height, 1.0))
return 0; // Pull back failed
}
// Now checking opposite sequence extremity
if (alongLinePosition < 0 || (distanceFromLine / alongLinePosition) > 0.5)
return 0;
found_pull_back:
a->m_head = i;
a->m_headLink = iLink;
}
return 1;
}
static void applyAxisConstraintVec(TransInfo *t, TransData *td, const float in[3], float out[3], float pvec[3])
{
copy_v3_v3(out, in);
if (!td && t->con.mode & CON_APPLY) {
mul_m3_v3(t->con.pmtx, out);
// With snap, a projection is alright, no need to correct for view alignment
if (!(!ELEM(t->tsnap.mode, SCE_SNAP_MODE_INCREMENT, SCE_SNAP_MODE_GRID) && activeSnap(t))) {
if (getConstraintSpaceDimension(t) == 2) {
if (out[0] != 0.0f || out[1] != 0.0f || out[2] != 0.0f) {
planeProjection(t, in, out);
}
}
else if (getConstraintSpaceDimension(t) == 1) {
float c[3];
if (t->con.mode & CON_AXIS0) {
copy_v3_v3(c, t->con.mtx[0]);
}
else if (t->con.mode & CON_AXIS1) {
copy_v3_v3(c, t->con.mtx[1]);
}
else if (t->con.mode & CON_AXIS2) {
copy_v3_v3(c, t->con.mtx[2]);
}
axisProjection(t, c, in, out);
}
}
postConstraintChecks(t, out, pvec);
}
}
//Junction Area expansion procedure
inline void JunctionArea::expandArea(unsigned int initial)
{
unsigned int a;
unsigned int curr, currLink;
unsigned int i, iLink, iNext;
m_jointsAbsorbed.push_back(initial);
currJSGraph->node(initial).setAttribute(JointSequenceGraph::REACHED); //Nodes absorbed gets signed
for (a = 0; a < m_jointsAbsorbed.size(); ++a) {
//Extract a joint from vector
curr = m_jointsAbsorbed[a];
for (currLink = 0; currLink < currJSGraph->getNode(curr).getLinksCount(); ++currLink) {
Sequence &s = *currJSGraph->node(curr).link(currLink);
if (s.m_graphHolder->getNode(s.m_head).getLink(s.m_headLink).getAccess()) {
//Expand into all nearby sequences, until a ROAD is found
i = s.m_head;
iLink = s.m_headLink;
for (; i != s.m_tail && !s.m_graphHolder->getNode(i).getLink(iLink)->hasAttribute(SkeletonArc::ROAD);
s.next(i, iLink))
;
//If the sequence has been completely run, include next joint in the
//expansion procedure AND sign it as 'REACHED'
if (i == s.m_tail) {
iNext = currJSGraph->getNode(curr).getLink(currLink).getNext();
if (!currJSGraph->node(iNext).hasAttribute(JointSequenceGraph::REACHED)) {
currJSGraph->node(iNext).setAttribute(JointSequenceGraph::REACHED);
m_jointsAbsorbed.push_back(iNext);
}
//Negate access to this sequence
s.m_graphHolder->node(s.m_tail).link(s.m_tailLink).setAccess(0);
s.m_graphHolder->node(s.m_head).link(s.m_headLink).setAccess(0);
} else {
//Initialize and copy the entering sequence found
m_enteringSequences.push_back(EnteringSequence(s));
m_enteringSequences.back().m_head = i;
m_enteringSequences.back().m_headLink = iLink;
//Initialize entering directions
iNext = s.m_graphHolder->getNode(i).getLink(iLink).getNext();
m_enteringSequences.back().m_direction =
planeProjection(*s.m_graphHolder->getNode(i) - *s.m_graphHolder->getNode(iNext));
m_enteringSequences.back().m_direction =
m_enteringSequences.back().m_direction * (1 / norm(m_enteringSequences.back().m_direction));
//Initialize entering height / slope
m_enteringSequences.back().m_height = s.m_graphHolder->getNode(i)->z;
//Also store pointer to link toward the joint at the end of sequence
m_enteringSequences.back().m_initialJoint = curr;
m_enteringSequences.back().m_outerLink = currLink;
}
}
}
}
}
static void applyObjectConstraintVec(TransInfo *t,
TransDataContainer *tc,
TransData *td,
const float in[3],
float out[3],
float pvec[3])
{
copy_v3_v3(out, in);
if (t->con.mode & CON_APPLY) {
if (!td) {
mul_m3_v3(t->con.pmtx, out);
const int dims = getConstraintSpaceDimension(t);
if (dims == 2) {
if (!is_zero_v3(out)) {
if (!isPlaneProjectionViewAligned(t)) {
planeProjection(t, in, out);
}
}
}
else if (dims == 1) {
float c[3];
if (t->con.mode & CON_AXIS0) {
copy_v3_v3(c, t->con.mtx[0]);
}
else if (t->con.mode & CON_AXIS1) {
copy_v3_v3(c, t->con.mtx[1]);
}
else if (t->con.mode & CON_AXIS2) {
copy_v3_v3(c, t->con.mtx[2]);
}
axisProjection(t, c, in, out);
}
postConstraintChecks(t, out, pvec);
copy_v3_v3(out, pvec);
}
else {
int i = 0;
out[0] = out[1] = out[2] = 0.0f;
if (t->con.mode & CON_AXIS0) {
out[0] = in[i++];
}
if (t->con.mode & CON_AXIS1) {
out[1] = in[i++];
}
if (t->con.mode & CON_AXIS2) {
out[2] = in[i++];
}
mul_m3_v3(td->axismtx, out);
if (t->flag & T_EDIT) {
mul_m3_v3(tc->mat3_unit, out);
}
}
}
}
inline bool JunctionArea::solveJunctionPosition()
{
std::vector<EnteringSequence>::iterator a;
double Sx2 = 0, Sy2 = 0, Sxy = 0;
TPointD P, v, b;
double h;
//Build preliminary sums for the linear system
for (a = m_enteringSequences.begin(); a != m_enteringSequences.end(); ++a) {
h = a->m_height;
v = a->m_direction;
P = planeProjection(*a->m_graphHolder->getNode(a->m_head));
//Height-weighted problem
Sx2 += sq(v.x) * h;
Sy2 += sq(v.y) * h;
Sxy += v.x * v.y * h;
b.x += h * (sq(v.y) * P.x - (v.x * v.y * P.y));
b.y += h * (sq(v.x) * P.y - (v.x * v.y * P.x));
}
//First check problem determinant
double det = Sx2 * Sy2 - sq(Sxy);
if (fabs(det) < 0.1)
return 0;
//Now construct linear system
TAffine M(Sx2 / det, Sxy / det, 0, Sxy / det, Sy2 / det, 0);
m_newJointPosition = M * b;
//Finally, check if J is too far from the line extensions of the entering
//sequences
for (a = m_enteringSequences.begin(); a != m_enteringSequences.end(); ++a) {
P = planeProjection(*a->m_graphHolder->getNode(a->m_head));
if (tdistance(m_newJointPosition, a->m_direction, P) > a->m_height * lineDistMul)
return 0;
}
return 1;
}
static void applyObjectConstraintVec(TransInfo *t, TransData *td, const float in[3], float out[3], float pvec[3])
{
copy_v3_v3(out, in);
if (t->con.mode & CON_APPLY) {
if (!td) {
mul_m3_v3(t->con.pmtx, out);
if (getConstraintSpaceDimension(t) == 2) {
if (out[0] != 0.0f || out[1] != 0.0f || out[2] != 0.0f) {
planeProjection(t, in, out);
}
}
else if (getConstraintSpaceDimension(t) == 1) {
float c[3];
if (t->con.mode & CON_AXIS0) {
copy_v3_v3(c, t->con.mtx[0]);
}
else if (t->con.mode & CON_AXIS1) {
copy_v3_v3(c, t->con.mtx[1]);
}
else if (t->con.mode & CON_AXIS2) {
copy_v3_v3(c, t->con.mtx[2]);
}
axisProjection(t, c, in, out);
}
postConstraintChecks(t, out, pvec);
copy_v3_v3(out, pvec);
}
else {
int i = 0;
out[0] = out[1] = out[2] = 0.0f;
if (t->con.mode & CON_AXIS0) {
out[0] = in[i++];
}
if (t->con.mode & CON_AXIS1) {
out[1] = in[i++];
}
if (t->con.mode & CON_AXIS2) {
out[2] = in[i++];
}
mul_m3_v3(td->axismtx, out);
if (t->flag & T_EDIT) {
mul_m3_v3(t->obedit_mat, out);
}
}
}
}
static void applyAxisConstraintVec(TransInfo *t,
TransDataContainer *UNUSED(tc),
TransData *td,
const float in[3],
float out[3],
float pvec[3])
{
copy_v3_v3(out, in);
if (!td && t->con.mode & CON_APPLY) {
mul_m3_v3(t->con.pmtx, out);
// With snap, a projection is alright, no need to correct for view alignment
if (!validSnap(t)) {
const int dims = getConstraintSpaceDimension(t);
if (dims == 2) {
if (!is_zero_v3(out)) {
if (!isPlaneProjectionViewAligned(t)) {
planeProjection(t, in, out);
}
}
}
else if (dims == 1) {
float c[3];
if (t->con.mode & CON_AXIS0) {
copy_v3_v3(c, t->con.mtx[0]);
}
else if (t->con.mode & CON_AXIS1) {
copy_v3_v3(c, t->con.mtx[1]);
}
else if (t->con.mode & CON_AXIS2) {
copy_v3_v3(c, t->con.mtx[2]);
}
axisProjection(t, c, in, out);
}
}
postConstraintChecks(t, out, pvec);
}
}
inline bool JunctionArea::checkShape()
{
std::vector<EnteringSequence>::iterator a, b;
unsigned int node, contour, first, last, n;
TPointD A, A1, B, B1, P, P1;
bool result = 1;
//First, sign all outgoing arcs' m_leftGeneratingNode as end of
//control procedure
for (a = m_enteringSequences.begin(); a != m_enteringSequences.end(); ++a) {
node = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getLeftGenerator();
contour = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getLeftContour();
(*currContourFamily)[contour][node].setAttribute(ContourNode::JR_RESERVED);
}
//Now, check shape
for (a = m_enteringSequences.begin(), b = m_enteringSequences.end() - 1;
a != m_enteringSequences.end(); b = a, ++a) {
//Initialize contour check
first = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getRightGenerator();
//last= b->m_graphHolder->getNode(b->m_head).getLink(b->m_headLink)
// ->getLeftGenerator();
contour = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getRightContour();
n = (*currContourFamily)[contour].size();
//Better this effective way to find the last
unsigned int numChecked = 0;
for (last = first; !(*currContourFamily)[contour][last].hasAttribute(ContourNode::JR_RESERVED) && numChecked < n; last = (last + 1) % n, ++numChecked)
;
//Security check
if (numChecked == n)
return 0;
A = planeProjection((*currContourFamily)[contour][first].m_position);
A1 = planeProjection((*currContourFamily)[contour][(first + 1) % n].m_position);
B = planeProjection((*currContourFamily)[contour][last].m_position);
B1 = planeProjection((*currContourFamily)[contour][(last + 1) % n].m_position);
for (node = first; !(*currContourFamily)[contour][node].hasAttribute(ContourNode::JR_RESERVED);
node = (node + 1) % n) {
P = planeProjection((*currContourFamily)[contour][node].m_position);
P1 = planeProjection((*currContourFamily)[contour][(node + 1) % n].m_position);
//EXPLANATION:
//Segment P-P1 must be included in fat lines passing for A-A1 or B-B1
result &=
(fabs(cross(P - A, normalize(A1 - A))) < a->m_height * shapeDistMul &&
fabs(cross(P1 - A, normalize(A1 - A))) < a->m_height * shapeDistMul)
||
(fabs(cross(P - B, normalize(B1 - B))) < b->m_height * shapeDistMul &&
fabs(cross(P1 - B, normalize(B1 - B))) < b->m_height * shapeDistMul);
}
}
//Finally, restore nodes attributes
for (a = m_enteringSequences.begin(); a != m_enteringSequences.end(); ++a) {
node = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getLeftGenerator();
contour = a->m_graphHolder->getNode(a->m_head).getLink(a->m_headLink)->getLeftContour();
(*currContourFamily)[contour][node].clearAttribute(ContourNode::JR_RESERVED);
}
return result;
}
