private Boolean find3DTransform(IList <node> locatedNodes, out double[,] T)
{
// T = trans*Quat2*Quat1*Scale*R*LHS
T = MatrixMath.Identity(4);
// if there are no nodes, just return the identity matrix
if (locatedNodes.Count == 0)
{
return(true);
}
/* move the first node into location by translation */
var transMatrix = MatrixMath.Identity(4);
var refPt = new double[3];
transMatrix[0, 3] = refPt[0] = locatedNodes[0].X;
transMatrix[1, 3] = refPt[1] = locatedNodes[0].Y;
transMatrix[2, 3] = refPt[2] = locatedNodes[0].Z;
if (locatedNodes.Count == 1)
{
T = transMatrix;
return(ValidTranslation(transMatrix.multiply(RegularizationMatrix, 4)));
}
// if there is just one node find the proper translation matrix (this T matrix * Regularization) and return
/* for 2 or more we first find scale factors needed. */
/* first, figure out how much to scale the shape */
var vHost = new[]
{
locatedNodes[1].X - refPt[0],
locatedNodes[1].Y - refPt[1],
locatedNodes[1].Z - refPt[2]
};
var vHost_length = vHost.norm2();
var vL = RegularizationMatrix.multiply(new[] { L.nodes[1].X, L.nodes[1].Y, L.nodes[1].Z, 1 }, 4);
var vL_length = vL[0];
var xScale = vHost_length / vL_length;
vL = new[] { 1.0, 0.0, 0.0 }; // turn vL into a unit vector - this is simply 1,0,0 since - after regularization
// the point is on the x-axisw
var axis = vL.crossProduct3(vHost);
double angle;
if (axis.Sum().sameCloseZero())
{
// if the two vectors are the same then the cross product will be all zeroes
axis = vHost;
angle = vHost[0] < 0 ? Math.PI : 0.0;
}
else
{
angle = Math.Acos(vHost[0] / vHost_length); // essentially, the dot product to find the angle
}
//now construct the quaternion for this rotation and multiply by T
double[,] quaternion1 = (angle.sameCloseZero()) ? MatrixMath.Identity(4) : makeQuaternion(axis, angle);
var scaleMatrix = MatrixMath.Identity(4);
scaleMatrix[0, 0] = xScale;
if (locatedNodes.Count == 2)
{
if (ValidScaling(_inverseRegMatrix.multiply(scaleMatrix, 4)))
{
T = scaleMatrix.multiply(RegularizationMatrix, 4);
}
else
{
scaleMatrix[1, 1] = scaleMatrix[2, 2] = xScale;
if (ValidScaling(_inverseRegMatrix.multiply(scaleMatrix, 4)))
{
T = scaleMatrix.multiply(RegularizationMatrix, 4);
}
else
{
return(false);
}
}
T = quaternion1.multiply(T, 4);
T = transMatrix.multiply(T, 4);
snapToIntValues(T);
return(ValidRotation(T, _inverseRegMatrix.multiply(scaleMatrix, 4)));
}
/* if there are 3 or more points, then we find a new Quaternion that will multiply
* the former result. In order to keep the second node in place, we use the vHost
* as the axis of rotation. */
// T = trans*Quat2*Quat1*Scale*R*LHS
axis = vHost;
var axisUnitVector = new[] { axis[0] / vHost_length, axis[1] / vHost_length, axis[2] / vHost_length };
// move the third L point (L.nodes[2] to the proper orientation. Note that it is not multiplied
// by translation since it is essentially the delta from the reference point - without translation, this
// is zero given the way Regularization puts the first node at {0,0,0}.
vL = RegularizationMatrix.multiply(new[] { L.nodes[2].X, L.nodes[2].Y, L.nodes[2].Z, 1 }, 4);
vL = scaleMatrix.multiply(vL, 4);
vL = quaternion1.multiply(vL, 4);
// dxAlongAxis is the dot project of where this point vL is projected to the the axis.
var dxAlongAxisL = vL[0] * axisUnitVector[0] + vL[1] * axisUnitVector[1] + vL[2] * axisUnitVector[2];
// set up a new host vector from the reference point, which is the location of the first node
vHost = new[]
{
locatedNodes[2].X - refPt[0],
locatedNodes[2].Y - refPt[1],
locatedNodes[2].Z - refPt[2]
};
// find the distance (dx) along the axis for this one as well.
// todo: what if dxAlongAxisL and dxAlongAxisHost are different? then I suppose there is a skew x w.r.t.y we could calculate
// var dxAlongAxisHost = vHost[0] * axisUnitVector[0] + vHost[1] * axisUnitVector[1] + vHost[2] * axisUnitVector[2];
// reformulate vL as the vector to the L-node from the axis.
vL = new[]
{
vL[0] - (dxAlongAxisL * axisUnitVector[0]),
vL[1] - (dxAlongAxisL * axisUnitVector[1]),
vL[2] - (dxAlongAxisL * axisUnitVector[2])
};
vL_length = vL.norm2(3);
// similarly reformulate vHost as the vector to the host-node from the axis.
vHost = new[]
{
vHost[0] - (dxAlongAxisL * axisUnitVector[0]),
vHost[1] - (dxAlongAxisL * axisUnitVector[1]),
vHost[2] - (dxAlongAxisL * axisUnitVector[2])
};
vHost_length = vHost.norm2();
// the ratio of these new lengths is the scale-Y term (well, this scale matrix if adulterated by the regularization, so it is
// not the true scale-y
var yScale = vHost_length / vL_length;
scaleMatrix[1, 1] = yScale;
// by using the dot-product equals cos(angle) identity, solve for the angle for the second quaternion operation
vL = new[] { vL[0] / vL_length, vL[1] / vL_length, vL[2] / vL_length };
vHost = new[] { vHost[0] / vHost_length, vHost[1] / vHost_length, vHost[2] / vHost_length };
var dot = vL[0] * vHost[0] + vL[1] * vHost[1] + vL[2] * vHost[2];
angle = dot >= 1.0 ? 0.0
: dot <= -1.0 ? Math.PI
: Math.Acos(dot);
var quaternion2 = (angle.sameCloseZero()) ? MatrixMath.Identity(4) : makeQuaternion(axis, angle);
if (locatedNodes.Count == 3)
{
if (ValidScaling(_inverseRegMatrix.multiply(scaleMatrix, 4)))
{
T = scaleMatrix.multiply(RegularizationMatrix, 4);
}
else
{
scaleMatrix[2, 2] = yScale;
if (ValidScaling(_inverseRegMatrix.multiply(scaleMatrix, 4)))
{
T = scaleMatrix.multiply(RegularizationMatrix, 4);
}
else
{
return(false);
}
}
T = quaternion1.multiply(T, 4);
T = quaternion2.multiply(T, 4);
T = transMatrix.multiply(T, 4);
snapToIntValues(T);
return(ValidRotation(T, _inverseRegMatrix.multiply(scaleMatrix, 4)));
}
// else, there are 4 or more
vL = scaleMatrix.multiply(RegularizationMatrix, 4).multiply(new[] { L.nodes[3].X, L.nodes[3].Y, L.nodes[3].Z, 1 }, 4);
vL_length = vL.norm2();
vHost = new[]
{
locatedNodes[3].X - locatedNodes[0].X,
locatedNodes[3].Y - locatedNodes[0].Y,
locatedNodes[3].Z - locatedNodes[0].Z
};
vHost_length = vHost.norm2();
var zScale = vHost_length / vL_length;
scaleMatrix[2, 2] = zScale;
T = scaleMatrix.multiply(RegularizationMatrix, 4);
T = quaternion1.multiply(T, 4);
T = quaternion2.multiply(T, 4);
T = transMatrix.multiply(T, 4);
snapToIntValues(T);
return(ValidScaling(_inverseRegMatrix.multiply(scaleMatrix, 4)) &&
ValidRotation(T, _inverseRegMatrix.multiply(scaleMatrix, 4)));
}
private Boolean General2DTransform(IList <node> locatedNodes, out double[,] T)
{
T = MatrixMath.Identity(4);
/* if there are no nodes, simply return the identity matrix */
if (locatedNodes.Count == 0)
{
return(true);
}
#region Variable Set-up
/* Variable Set-up: This seems a little verbose, but it is necessary
* to ease the calculations later and to avoid compile errors. */
double x1, x2, x3, x4, y1, y2, y3, y4;
x2 = x3 = x4 = y2 = y3 = y4 = 0;
double tx, ty, wX, wY, a, b, c, d;
tx = ty = wX = wY = a = b = c = d = 0;
double k1, k2, k3, k4;
k1 = k2 = k3 = k4 = 0;
double u3, u4, v3, v4;
u3 = u4 = v3 = v4 = 0;
/* This x1 and y1 are matched with the position of L.nodes[0].X and .Y,
* which, given the Regularization concept, is effectively at 0,0. This is
* what Regularization does. It's as if the first node in L is moved to zero
* without loss of generality, and all the other nodes are translated accord-
* ingly. So, u1 = v1 = 0. */
x1 = locatedNodes[0].X;
y1 = locatedNodes[0].Y;
if (locatedNodes.Count >= 2)
{
x2 = locatedNodes[1].X;
y2 = locatedNodes[1].Y;
/* Given regularization, this second point is scaled and rotated to 1, 0. */
}
if (locatedNodes.Count >= 3)
{
x3 = locatedNodes[2].X;
y3 = locatedNodes[2].Y;
var temp = new[] { L.nodes[2].X, L.nodes[2].Y, 0.0, 1.0 };
temp = RegularizationMatrix.multiply(temp, 4);
u3 = temp[0];
v3 = temp[1];
}
if (locatedNodes.Count >= 4)
{
x4 = locatedNodes[3].X;
y4 = locatedNodes[3].Y;
var temp = new[] { L.nodes[3].X, L.nodes[3].Y, 1.0 };
temp = RegularizationMatrix.multiply(temp, 3);
u4 = temp[0];
v4 = temp[1];
}
#endregion
// set values for tx, and ty
tx = x1;
ty = y1;
#region Calculate Projection Terms
if ((locatedNodes.Count <= 3) || ((v3 * v4).sameCloseZero()))
{
wX = wY = 0;
}
else
{
//calculate intermediate values used only in this class or method
//k1 = (u4 * (y4 - y2) / v4 - u3 * (y3 - y2) / v3); //(Equation 3 of program)
k1 = u4 * v3 * (y4 - y2) - u3 * v4 * (y3 - y2);
if (k1.sameCloseZero())
{
k1 = 0;
}
else
{
k1 /= v3 * v4;
}
//k2 = (y3 - y2 * u3 + ty * u3 - ty) / v3 + (-y4 - ty * u4 + y2 * u4 + ty) / v4; //(Equation 4 of program)
k2 = v4 * (y3 - y2 * u3 + ty * u3 - ty) + v3 * (-y4 - ty * u4 + y2 * u4 + ty);
if (k2.sameCloseZero())
{
k2 = 0;
}
else
{
k2 /= v3 * v4;
}
//k3 = (u3 * (x3 - x2) / v3 - u4 * (x4 - x2) / v4);
k3 = u3 * v4 * (x3 - x2) - u4 * v3 * (x4 - x2);
if (k3.sameCloseZero())
{
k3 = 0;
}
else
{
k3 /= v3 * v4;
}
//k4 = (x4 - x2 * u4 + tx * u4 - tx) / v4 - (x3 + tx * u3 - x2 * u3 - tx) / v3;
k4 = v3 * (x4 - x2 * u4 + tx * u4 - tx) - v4 * (x3 + tx * u3 - x2 * u3 - tx);
if (k4.sameCloseZero())
{
k4 = 0;
}
else
{
k4 /= v3 * v4;
}
//calculate wY, and wX
wY = (k1 * k4) - (k2 * k3);
if (wY.sameCloseZero())
{
wY = 0;
}
else
{
wY /= k3 * (y3 - y4) + k1 * (x3 - x4); //(Equation 7 of program)
}
wX = wY * (y3 - y4) + k2;
if (wX.sameCloseZero())
{
wX = 0;
}
else
{
wX /= k1; //is (Equation 8 of program) which is rewritten for program's accuracy
}
}
#endregion
#region Calculate rotate, scale, skew terms
if (locatedNodes.Count <= 1)
{
a = d = 1;
b = c = 0;
}
else
{
// calculate a
a = x2 * (wX + 1) - tx;
//calculate c
c = y2 * (wX + 1) - ty;
if ((locatedNodes.Count <= 2) || (LnodesAreCollinear()))
{
/* in order for the validTransform to function, b and d are set as
* if there is a rotation as opposed to a Skew in X. It is likely that
* isotropic transformations like rotation are more often intended than skews. */
// var theta = Math.Atan2(-c, a);
b = -c;
d = a;
}
else
{
//calculate b
b = x3 * (wX * u3 + wY * v3 + 1) - a * u3 - tx;
if (b.sameCloseZero())
{
b = 0;
}
else
{
b /= v3;
}
//calculate d
d = y3 * (wX * u3 + wY * v3 + 1) - c * u3 - ty;
if (d.sameCloseZero())
{
d = 0;
}
else
{
d /= v3;
}
}
}
#endregion
T[0, 0] = a;
T[0, 1] = b;
T[0, 3] = tx;
T[1, 0] = c;
T[1, 1] = d;
T[1, 3] = ty;
T[3, 0] = wX;
T[3, 1] = wY;
T[3, 3] = 1;
T = T.multiply(RegularizationMatrix, 4);
T[0, 0] /= T[3, 3];
T[0, 1] /= T[3, 3];
T[0, 2] = 0.0;
T[0, 3] /= T[3, 3];
T[1, 0] /= T[3, 3];
T[1, 1] /= T[3, 3];
T[1, 2] = 0.0;
T[1, 3] /= T[3, 3];
T[2, 0] = 0.0;
T[2, 1] = 0.0;
T[2, 2] = 1.0;
T[2, 3] = 0.0;
T[3, 0] /= T[3, 3];
T[3, 1] /= T[3, 3];
T[3, 2] = 0.0;
T[3, 3] = 1;
snapToIntValues(T);
//if (RestrictToNodeShapeMatch && T[0,0]==1 && T[)
return(validTransform(T));
}
