/// <summary>Transform a Normal by the (transpose of the) given Matrix</summary>
/// <remarks>
/// This version doesn't calculate the inverse matrix.
/// Use this version if you already have the inverse of the desired transform to hand
/// </remarks>
/// <param name="norm">The normal to transform</param>
/// <param name="invMat">The inverse of the desired transformation</param>
/// <param name="result">The transformed normal</param>
public static void TransformNormalInverse(ref Vector3d norm, ref Matrix4d invMat, out Vector3d result)
{
result.X = norm.X * invMat.Row0.X +
norm.Y * invMat.Row0.Y +
norm.Z * invMat.Row0.Z;
result.Y = norm.X * invMat.Row1.X +
norm.Y * invMat.Row1.Y +
norm.Z * invMat.Row1.Z;
result.Z = norm.X * invMat.Row2.X +
norm.Y * invMat.Row2.Y +
norm.Z * invMat.Row2.Z;
}
/// <summary>Transform a direction vector by the given Matrix
/// Assumes the matrix has a bottom row of (0,0,0,1), that is the translation part is ignored.
/// </summary>
/// <remarks>
/// It is incorrect to call this method passing the same variable for
/// <paramref name="result"/> as for <paramref name="vec"/>.
/// </remarks>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void TransformVector(ref Vector3d vec, ref Matrix4d mat, out Vector3d result)
{
result.X = vec.X * mat.Row0.X +
vec.Y * mat.Row1.X +
vec.Z * mat.Row2.X;
result.Y = vec.X * mat.Row0.Y +
vec.Y * mat.Row1.Y +
vec.Z * mat.Row2.Y;
result.Z = vec.X * mat.Row0.Z +
vec.Y * mat.Row1.Z +
vec.Z * mat.Row2.Z;
}
/// <summary>Transform a Position by the given Matrix</summary>
/// <param name="pos">The position to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed position</param>
public static void TransformPosition(ref Vector3d pos, ref Matrix4d mat, out Vector3d result)
{
result.X = pos.X * mat.Row0.X +
pos.Y * mat.Row1.X +
pos.Z * mat.Row2.X +
mat.Row3.X;
result.Y = pos.X * mat.Row0.Y +
pos.Y * mat.Row1.Y +
pos.Z * mat.Row2.Y +
mat.Row3.Y;
result.Z = pos.X * mat.Row0.Z +
pos.Y * mat.Row1.Z +
pos.Z * mat.Row2.Z +
mat.Row3.Z;
}
public void initInverseVectorsOPENTK()
{
Scientrace.Vector x = this.u;
Scientrace.Vector y = this.v;
Scientrace.Vector z = this.w;
OpenTK.Matrix4d m = new OpenTK.Matrix4d(
x.x, x.y, x.z, 0,
y.x, y.y, y.z, 0,
z.x, z.y, z.z, 0,
0, 0, 0, 1);
m.Invert();
this.ui = new Scientrace.Vector(m.M11, m.M12, m.M13);
this.vi = new Scientrace.Vector(m.M21, m.M22, m.M23);
this.wi = new Scientrace.Vector(m.M31, m.M32, m.M33);
}
/// <summary>Constructs left Quaterniond from the given matrix. Only contains rotation information.</summary>
/// <param name="matrix">The matrix for the components of the Quaterniond.</param>
public Quaterniond(ref Matrix4d matrix)
{
double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d);
W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2;
if (matrix[2, 1] - matrix[1, 2] < 0)
{
X = -X;
}
if (matrix[0, 2] - matrix[2, 0] < 0)
{
Y = -Y;
}
if (matrix[1, 0] - matrix[0, 1] < 0)
{
Z = -Z;
}
}
/// <summary>Transform a Vector by the given Matrix</summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void Transform(ref Vector4d vec, ref Matrix4d mat, out Vector4d result)
{
result.X = vec.X * mat.Row0.X +
vec.Y * mat.Row1.X +
vec.Z * mat.Row2.X +
vec.W * mat.Row3.X;
result.Y = vec.X * mat.Row0.Y +
vec.Y * mat.Row1.Y +
vec.Z * mat.Row2.Y +
vec.W * mat.Row3.Y;
result.Z = vec.X * mat.Row0.Z +
vec.Y * mat.Row1.Z +
vec.Z * mat.Row2.Z +
vec.W * mat.Row3.Z;
result.W = vec.X * mat.Row0.W +
vec.Y * mat.Row1.W +
vec.Z * mat.Row2.W +
vec.W * mat.Row3.W;
}
public TorusKnot(int pathsteps, int shapevertices, double radius, int p, int q, int TexCount)
: base( )
{
Trace.Assert(pathsteps >= MINPathSteps, "A Path must have at least " + MINPathSteps + " Steps to form a volume.");
Trace.Assert(shapevertices >= MINShapeVertices, "A Shape must contain at least " + MINShapeVertices + " Vertices to be considered valid and create a volume.");
Trace.Assert(TexCount > 1, "at least 1 Texture set is required.");
PrimitiveMode = OpenTK.Graphics.OpenGL.BeginMode.TriangleStrip;
Vector3d[] PathPositions = new Vector3d[pathsteps];
#region Find the center Points for each step on the path
for (int i = 0; i < pathsteps; i++)
{
double Angle = (i / (double)pathsteps) * TwoPi;
double AngleTimesP = Angle * p;
double AngleTimesQ = Angle * q;
double r = (0.5 * (2.0 + System.Math.Sin(AngleTimesQ)));
PathPositions[i] = new Vector3d((r * System.Math.Cos(AngleTimesP)),
(r * System.Math.Cos(AngleTimesQ)),
(r * System.Math.Sin(AngleTimesP)));
}
#endregion Find the center Points for each step on the path
#region Find the Torus length
Vector3d result;
double[] Lengths = new double[pathsteps];
Vector3d.Subtract(ref PathPositions[pathsteps - 1], ref PathPositions[0], out result);
Lengths[0] = result.Length;
double TotalLength = result.Length;
for (int i = 1; i < pathsteps; i++) // skipping
{
Vector3d.Subtract(ref PathPositions[i - 1], ref PathPositions[i], out result);
Lengths[i] = result.Length;
TotalLength += result.Length;
}
Trace.WriteLine("the TorusKnot's length is: " + TotalLength + " ");
#endregion Find the Torus length
VertexArray = new VertexT2dN3dV3d[pathsteps * shapevertices];
#region Loft a circle Shape along the path
double TwoPiThroughVert = TwoPi / shapevertices; // precalc for reuse
for (uint i = 0; i < pathsteps; i++)
{
Vector3d last, next, normal, tangent;
if (i == pathsteps - 1)
{
next = PathPositions[0];
}
else
{
next = PathPositions[i + 1];
}
if (i == 0)
{
last = PathPositions[pathsteps - 1];
}
else
{
last = PathPositions[i - 1];
}
Vector3d.Subtract(ref next, ref last, out tangent); // Guesstimate tangent
tangent.Normalize();
Vector3d.Add(ref next, ref last, out normal); // Approximate N
normal.Normalize();
Vector3d.Multiply(ref normal, radius, out normal); // scale the shape to desired radius
for (uint j = 0; j < shapevertices; j++)
{
uint index = i * (uint)shapevertices + j;
// Create a point on the plane and rotate it
OpenTK.Matrix4d RotationMatrix = OpenTK.Matrix4d.Rotate(tangent, -(j * TwoPiThroughVert));
OpenTK.Vector3d point = OpenTK.Vector3d.TransformVector(normal, RotationMatrix);
OpenTK.Vector3d.Add(ref PathPositions[i], ref point, out VertexArray[index].Position);
// Since the used shape is a circle, the Vertex normal's heading is easy to find
OpenTK.Vector3d.Subtract(ref VertexArray[index].Position, ref PathPositions[i], out VertexArray[index].Normal);
VertexArray[index].Normal.Normalize();
// just generate some semi-useful UVs to fill blanks
VertexArray[index].TexCoord = new OpenTK.Vector2d((double)(i / TotalLength / TexCount), j / (shapevertices - 1.0));
}
}
#endregion Loft a circle Shape along the path
PathPositions = null; // not needed anymore
uint currentindex = 0;
#region Build a Triangle strip from the Vertices
IndexArray = new uint[pathsteps * (shapevertices * 2 + 2)]; // 2 triangles per vertex, +2 due to added degenerate triangles
for (uint i = 0; i < pathsteps; i++)
{
uint RowCurrent = i * (uint)shapevertices;
uint RowBelow;
if (i == pathsteps - 1)
{
RowBelow = 0; // for the last row, the first row is the following
}
else
{
RowBelow = (i + 1) * (uint)shapevertices;
}
// new ring begins here
for (uint j = 0; j < shapevertices; j++)
{
IndexArray[currentindex++] = RowCurrent + j;
IndexArray[currentindex++] = RowBelow + j;
}
// ring ends here, repeat first 2 vertices to insert 2 degenerate triangles to reach following ring
IndexArray[currentindex++] = RowCurrent;
IndexArray[currentindex++] = RowBelow;
}
#endregion Build a Triangle strip from the Vertices
}
public static Vector3d TransformPosition(Vector3d pos, Matrix4d mat)
{
return(new Vector3d(Vector3d.Dot(pos, new Vector3d(mat.Column0)) + mat.Row3.X, Vector3d.Dot(pos, new Vector3d(mat.Column1)) + mat.Row3.Y, Vector3d.Dot(pos, new Vector3d(mat.Column2)) + mat.Row3.Z));
}
public static Vector3d TransformNormalInverse(Vector3d norm, Matrix4d invMat)
{
return(new Vector3d(Vector3d.Dot(norm, new Vector3d(invMat.Row0)), Vector3d.Dot(norm, new Vector3d(invMat.Row1)), Vector3d.Dot(norm, new Vector3d(invMat.Row2))));
}
public static void TransformNormal(ref Vector3d norm, ref Matrix4d mat, out Vector3d result)
{
Matrix4d invMat = Matrix4d.Invert(mat);
Vector3d.TransformNormalInverse(ref norm, ref invMat, out result);
}
public static Vector3d TransformNormal(Vector3d norm, Matrix4d mat)
{
mat.Invert();
return(Vector3d.TransformNormalInverse(norm, mat));
}
public static Vector3d TransformVector(Vector3d vec, Matrix4d mat)
{
return(new Vector3d(Vector3d.Dot(vec, new Vector3d(mat.Column0)), Vector3d.Dot(vec, new Vector3d(mat.Column1)), Vector3d.Dot(vec, new Vector3d(mat.Column2))));
}
/// <summary>
/// Transform a Vector3d by the given Matrix, and project the resulting Vector4 back to a Vector3
/// </summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed vector</returns>
public static Vector3d TransformPerspective(Vector3d vec, Matrix4d mat)
{
Vector4d h = Transform(vec, mat);
return(new Vector3d(h.X / h.W, h.Y / h.W, h.Z / h.W));
}
public bool Update()
{
/*
* The solution is based on
* Berthold K. P. Horn (1987),
* "Closed-form solution of absolute orientation using unit quaternions,"
* Journal of the Optical Society of America A, 4:629-642
*/
// Original python implementation by David G. Gobbi
if (this.SourceLandmarks == null || this.TargetLandmarks == null)
{
//Identity Matrix
this.Matrix = new Matrix4d(Vector4d.UnitX, Vector4d.UnitY, Vector4d.UnitZ, Vector4d.UnitW);
return(false);
}
// --- compute the necessary transform to match the two sets of landmarks ---
int N_PTS = this.SourceLandmarks.Count;
if (N_PTS != this.TargetLandmarks.Count)
{
System.Diagnostics.Debug.WriteLine("Error: Source and Target Landmarks contain a different number of points");
return(false);
}
// -- if no points, stop here
if (N_PTS == 0)
{
//Identity Matrix
this.Matrix = new Matrix4d(Vector4d.UnitX, Vector4d.UnitY, Vector4d.UnitZ, Vector4d.UnitW);
return(false);
}
double[] source_centroid = { 0, 0, 0 };
double[] target_centroid = { 0, 0, 0 };
FindCentroids(N_PTS, source_centroid, target_centroid);
///-------------------------------
// -- if only one point, stop right here
if (N_PTS == 1)
{
this.Matrix = new Matrix4d(Vector4d.UnitX, Vector4d.UnitY, Vector4d.UnitZ, Vector4d.UnitW);
Matrix[0, 3] = target_centroid[0] - source_centroid[0];
this.Matrix[1, 3] = target_centroid[1] - source_centroid[1];
this.Matrix[2, 3] = target_centroid[2] - source_centroid[2];
return(true);
}
// -- build the 3x3 matrix M --
double[,] M = new double[3, 3];
double[,] AAT = new double[3, 3];
for (int i = 0; i < 3; i++)
{
AAT[i, 0] = M[i, 0] = 0.0F; // fill M with zeros
AAT[i, 1] = M[i, 1] = 0.0F;
AAT[i, 2] = M[i, 2] = 0.0F;
}
int pt;
for (pt = 0; pt < N_PTS; pt++)
{
double scale = 0F;
double[,] N = CreateMatrixForDiag(pt, source_centroid, target_centroid, M, ref scale);
double[,] eigenvectorData = new double[4, 4];
//double *eigenvectors[4],eigenvalues[4];
double[,] eigenvectors = new double[4, 4];
double[] eigenvalues = new double[4];
//for (int i = 0; i < 4; i++)
//{
// for (int j = 0; j < 4; j++)
// {
// eigenvectors[i, j] = eigenvectorData[i, j];
// }
//}
MathUtils.JacobiN(N, 4, eigenvalues, eigenvectors);
//returns this.Matrix
CreateMatrixOutOfDiagonalizationResult(eigenvectors, eigenvalues, N_PTS, source_centroid, target_centroid, scale);
//this.Matrix.Modified();
}
return(true);
}
public static OpenTK.Matrix4 Convert(OpenTK.Matrix4d mat)
{
return(new OpenTK.Matrix4((float)mat.M11, (float)mat.M12, (float)mat.M13, (float)mat.M14, (float)mat.M21, (float)mat.M22, (float)mat.M23,
(float)mat.M24, (float)mat.M31, (float)mat.M32, (float)mat.M33, (float)mat.M34, (float)mat.M41, (float)mat.M42, (float)mat.M43, (float)mat.M44));
}
/// <summary>
/// Constructs a new instance.
/// </summary>
/// <param name="matrix">A Matrix4d to take the upper-left 3x3 from.</param>
public Matrix3d(Matrix4d matrix)
{
Row0 = matrix.Row0.Xyz;
Row1 = matrix.Row1.Xyz;
Row2 = matrix.Row2.Xyz;
}
public void SetProjection(CalibrationResult calib)
{
parent.MakeCurrent();
var Width = parent.Width;
var Height = parent.Height;
GL.Viewport(0,0, Width, Height);
var rt = calib.Extrinsic.ExtrinsicMatrix.Data;
var extrin = new OpenTK.Matrix4d(
rt[0, 0], rt[1, 0], rt[2, 0], 0,
rt[0, 1], rt[1, 1], rt[2, 1], 0,
rt[0, 2], rt[1, 2], rt[2, 2], 0,
rt[0, 3], rt[1, 3], rt[2, 3], 1);
GL.MatrixMode(MatrixMode.Projection);
GL.LoadMatrix(ref extrin);
GL.MatrixMode(MatrixMode.Modelview);
GL.LoadIdentity();
var intrin = calib.Intrinsic.IntrinsicMatrix.Data;
var dist = calib.Intrinsic.DistortionCoeffs.Data;
F = new Vector2((float)intrin[0, 0], (float)intrin[1, 1]);
C = new Vector2((float)intrin[0, 2], (float)intrin[1, 2]);
K = new Matrix4(
(float)dist[0, 0], (float)dist[1, 0], (float)dist[2, 0], (float)dist[3, 0],
(float)dist[4, 0], (float)dist[5, 0], (float)dist[6, 0], (float)dist[7, 0],
0,0,0,0,
0,0,0,0);
}
public static void Transform(ref Vector4d vec, ref Matrix4d mat, out Vector4d result)
{
result = new Vector4d(vec.X * mat.Row0.X + vec.Y * mat.Row1.X + vec.Z * mat.Row2.X + vec.W * mat.Row3.X, vec.X * mat.Row0.Y + vec.Y * mat.Row1.Y + vec.Z * mat.Row2.Y + vec.W * mat.Row3.Y, vec.X * mat.Row0.Z + vec.Y * mat.Row1.Z + vec.Z * mat.Row2.Z + vec.W * mat.Row3.Z, vec.X * mat.Row0.W + vec.Y * mat.Row1.W + vec.Z * mat.Row2.W + vec.W * mat.Row3.W);
}
/// <summary>Returns left matrix for this Quaterniond.</summary>
public void Matrix4d(out Matrix4d result)
{
// TODO Expand
result = new Matrix4d(ref this);
}
//----------------------------------------------------------------------------
public LandmarkTransform()
{
Matrix = new Matrix4d();
}
/// <summary>Transform a Vector by the given Matrix</summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void Transform(ref Vector3d vec, ref Matrix4d mat, out Vector4d result)
{
Vector4d v4 = new Vector4d(vec.X, vec.Y, vec.Z, 1.0f);
Vector4d.Transform(ref v4, ref mat, out result);
}
public void initInverseVectorsOPENTK()
{
Scientrace.Vector x = this.u;
Scientrace.Vector y = this.v;
Scientrace.Vector z = this.w;
OpenTK.Matrix4d m = new OpenTK.Matrix4d(
x.x, x.y, x.z, 0,
y.x, y.y, y.z, 0,
z.x, z.y, z.z, 0,
0, 0, 0, 1);
m.Invert();
this.ui = new Scientrace.Vector(m.M11, m.M12, m.M13);
this.vi = new Scientrace.Vector(m.M21, m.M22, m.M23);
this.wi = new Scientrace.Vector(m.M31, m.M32, m.M33);
}
