public Frame3d(Vector3d origin, Quaterniond orientation)
{
rotation = orientation;
this.origin = origin;
}
public double Dot(Quaterniond q2)
{
return(x * q2.x + y * q2.y + z * q2.z + w * q2.w);
}
/// <summary>
/// Solve for Translate/Rotate update, based on current From and To
/// Note: From and To are invalidated, will need to be re-computed after calling this
/// </summary>
void update_transformation()
{
int N = From.Length;
// normalize weights
double wSum = 0;
for (int i = 0; i < N; ++i)
{
wSum += Weights[i];
}
double wSumInv = 1.0 / wSum;
// compute means
Vector3d MeanX = Vector3d.Zero;
Vector3d MeanY = Vector3d.Zero;
for (int i = 0; i < N; ++i)
{
MeanX += (Weights[i] * wSumInv) * From[i];
MeanY += (Weights[i] * wSumInv) * To[i];
}
// subtract means
for (int i = 0; i < N; ++i)
{
From[i] -= MeanX;
To[i] -= MeanY;
}
// construct matrix 3x3 Matrix From*Transpose(To)
// (vectors are columns)
double[] M = new double[9];
for (int k = 0; k < 3; ++k)
{
int r = 3 * k;
for (int i = 0; i < N; ++i)
{
double lhs = (Weights[i] * wSumInv) * From[i][k];
M[r + 0] += lhs * To[i].x;
M[r + 1] += lhs * To[i].y;
M[r + 2] += lhs * To[i].z;
}
}
// compute SVD of M
SingularValueDecomposition svd = new SingularValueDecomposition(3, 3, 100);
uint ok = svd.Solve(M, -1); // sort in decreasing order, like Eigen
Debug.Assert(ok < 9999999);
double[] U = new double[9], V = new double[9], Tmp = new double[9];;
svd.GetU(U);
svd.GetV(V);
// this is our rotation update
double[] RotUpdate = new double[9];
// U*V gives us desired rotation
double detU = MatrixUtil.Determinant3x3(U);
double detV = MatrixUtil.Determinant3x3(V);
if (detU * detV < 0)
{
double[] S = MatrixUtil.MakeDiagonal3x3(1, 1, -1);
MatrixUtil.Multiply3x3(V, S, Tmp);
MatrixUtil.Transpose3x3(U);
MatrixUtil.Multiply3x3(Tmp, U, RotUpdate);
}
else
{
MatrixUtil.Transpose3x3(U);
MatrixUtil.Multiply3x3(V, U, RotUpdate);
}
// convert matrix to quaternion
Matrix3d RotUpdateM = new Matrix3d(RotUpdate);
Quaterniond RotUpdateQ = new Quaterniond(RotUpdateM);
// [TODO] is this right? We are solving for a translation and
// rotation of the current From points, but when we fold these
// into Translation & Rotation variables, we have essentially
// changed the order of operations...
// figure out translation update
Vector3d TransUpdate = MeanY - RotUpdateQ * MeanX;
Translation += TransUpdate;
// and rotation
Rotation = RotUpdateQ * Rotation;
// above was ported from this code...still not supporting weights though.
// need to figure out exactly what this code does (like, what does
// transpose() do to a vector??)
// /// Normalize weight vector
//Eigen::VectorXd w_normalized = w/w.sum();
///// De-mean
//Eigen::Vector3d X_mean, Y_mean;
//for(int i=0; i<3; ++i) {
// X_mean(i) = (X.row(i).array()*w_normalized.transpose().array()).sum();
// Y_mean(i) = (Y.row(i).array()*w_normalized.transpose().array()).sum();
//Eigen::Matrix3d sigma = X * w_normalized.asDiagonal() * Y.transpose();
}
/// <summary>
/// Interpolate between two frames - Lerp for origin, Slerp for rotation
/// </summary>
public static Frame3d Interpolate(Frame3d f1, Frame3d f2, double t)
{
return(new Frame3d(
Vector3d.Lerp(f1.origin, f2.origin, t),
Quaterniond.Slerp(f1.rotation, f2.rotation, t)));
}
public Quaterniond(Quaterniond p, Quaterniond q, double t)
{
x = y = z = 0; w = 1;
SetToSlerp(p, q, t);
}
public static Vector3d Rotate(Vector3d pos, Vector3d origin, Quaterniond rotation)
{
return(rotation * (pos - origin) + origin);
}
public void AlignAxis(int nAxis, Vector3d vTo)
{
Quaterniond rot = Quaterniond.FromTo(GetAxis(nAxis), vTo);
Rotate(rot);
}
public static Quaterniond FromToConstrained(Vector3d vFrom, Vector3d vTo, Vector3d vAround)
{
double fAngle = MathUtil.PlaneAngleSignedD(vFrom, vTo, vAround);
return(Quaterniond.AxisAngleD(vAround, fAngle));
}
/// <summary>
/// this rotates the frame around its own axes, rather than around the world axes,
/// which is what Rotate() does. So, RotateAroundAxis(AxisAngleD(Z,180)) is equivalent
/// to Rotate(AxisAngleD(My_AxisZ,180)).
/// </summary>
public void RotateAroundAxes(Quaterniond q)
{
rotation = rotation * q;
}
public Frame3d RotatedAround(Vector3d point, Quaterniond q)
{
Vector3d dv = q * (this.origin - point);
return(new Frame3d(point + dv, q * this.rotation));
}
public Frame3d Rotated(Quaterniond q)
{
return(new Frame3d(this.origin, q * this.rotation));
}
public Frame3d(Frame3d copy)
{
this.rotation = copy.rotation;
this.origin = copy.origin;
}
public void Rotate(Quaterniond q)
{
rotation = q * rotation;
}
public static Quaterniond Inverse(Quaterniond q)
{
return(q.Inverse());
}
public Frame3d(Vector3d origin)
{
rotation = Quaterniond.Identity;
this.origin = origin;
}
public Quaterniond(Quaterniond q2)
{
x = q2.x; y = q2.y; z = q2.z; w = q2.w;
}
public Frame3d(Vector3d origin, Vector3d setZ)
{
rotation = Quaterniond.FromTo(Vector3d.AxisZ, setZ);
this.origin = origin;
}
public static Quaterniond Slerp(Quaterniond p, Quaterniond q, double t)
{
return(new Quaterniond(p, q, t));
}
///<summary> Map quaternion *into* local coordinates of Frame </summary>
public Quaterniond ToFrame(ref Quaterniond q)
{
return(Quaterniond.Inverse(this.rotation) * q);
}
public static Frame3f Rotate(Frame3f f, Vector3d origin, Quaterniond rotation)
{
f.Rotate((Quaternionf)rotation);
f.Origin = (Vector3f)Rotate(f.Origin, origin, rotation);
return(f);
}
/// <summary> Map quaternion *from* local frame coordinates into "world" coordinates </summary>
public Quaterniond FromFrame(ref Quaterniond q)
{
return(this.rotation * q);
}
// ported from WildMagic5 Wm5ContBox3.cpp::MergeBoxes
public static Box3d Merge(ref Box3d box0, ref Box3d box1)
{
// Construct a box that contains the input boxes.
var box = new Box3d();
// The first guess at the box center. This value will be updated later
// after the input box vertices are projected onto axes determined by an
// average of box axes.
box.Center = 0.5 * (box0.Center + box1.Center);
// A box's axes, when viewed as the columns of a matrix, form a rotation
// matrix. The input box axes are converted to quaternions. The average
// quaternion is computed, then normalized to unit length. The result is
// the slerp of the two input quaternions with t-value of 1/2. The result
// is converted back to a rotation matrix and its columns are selected as
// the merged box axes.
Quaterniond q0 = new Quaterniond(), q1 = new Quaterniond();
var rot0 = new Matrix3d(ref box0.AxisX, ref box0.AxisY, ref box0.AxisZ, false);
q0.SetFromRotationMatrix(ref rot0);
var rot1 = new Matrix3d(ref box1.AxisX, ref box1.AxisY, ref box1.AxisZ, false);
q1.SetFromRotationMatrix(ref rot1);
if (q0.Dot(q1) < 0)
{
q1 = -q1;
}
Quaterniond q = q0 + q1;
double invLength = 1.0 / Math.Sqrt(q.Dot(q));
q = q * invLength;
Matrix3d q_mat = q.ToRotationMatrix();
box.AxisX = q_mat.Column(0); box.AxisY = q_mat.Column(1); box.AxisZ = q_mat.Column(2); //q.ToRotationMatrix(box.Axis);
// Project the input box vertices onto the merged-box axes. Each axis
// D[i] containing the current center C has a minimum projected value
// min[i] and a maximum projected value max[i]. The corresponding end
// points on the axes are C+min[i]*D[i] and C+max[i]*D[i]. The point C
// is not necessarily the midpoint for any of the intervals. The actual
// box center will be adjusted from C to a point C' that is the midpoint
// of each interval,
// C' = C + sum_{i=0}^2 0.5*(min[i]+max[i])*D[i]
// The box extents are
// e[i] = 0.5*(max[i]-min[i])
int i, j;
double dot;
var vertex = new Vector3d[8];
Vector3d pmin = Vector3d.Zero;
Vector3d pmax = Vector3d.Zero;
box0.ComputeVertices(vertex);
for (i = 0; i < 8; ++i)
{
Vector3d diff = vertex[i] - box.Center;
for (j = 0; j < 3; ++j)
{
dot = box.Axis(j).Dot(ref diff);
if (dot > pmax[j])
{
pmax[j] = dot;
}
else if (dot < pmin[j])
{
pmin[j] = dot;
}
}
}
box1.ComputeVertices(vertex);
for (i = 0; i < 8; ++i)
{
Vector3d diff = vertex[i] - box.Center;
for (j = 0; j < 3; ++j)
{
dot = box.Axis(j).Dot(ref diff);
if (dot > pmax[j])
{
pmax[j] = dot;
}
else if (dot < pmin[j])
{
pmin[j] = dot;
}
}
}
// [min,max] is the axis-aligned box in the coordinate system of the
// merged box axes. Update the current box center to be the center of
// the new box. Compute the extents based on the new center.
for (j = 0; j < 3; ++j)
{
box.Center += (0.5 * (pmax[j] + pmin[j])) * box.Axis(j);
box.Extent[j] = 0.5 * (pmax[j] - pmin[j]);
}
return(box);
}
/// <summary>
/// Create new Vector3d representing an Euler angles for a Quaternionf rotation.
/// </summary>
/// <param name="quat">Quaternion</param>
/// <returns>Vector3 representing Euler angles rotation</returns>
public static Vector3d ToEuler(Quaterniond quat)
{
return(quat.ToEuler());
}
