/// <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); }