Exemple #1
0
        /// <summary>
        /// Calculates the inverse matrix using Householder transformations
        /// </summary>
        public static M44d QrInverse(this M44d mat)
        {
            var qr   = new Matrix <double>((double[])mat, 4, 4);
            var diag = qr.QrFactorize();

            return((M44d)(qr.QrInverse(diag).Data));
        }
        /// <summary>
        /// Transforms a <see cref="M34d"/> to a <see cref="M33d"/> by deleting the
        /// specified row and column. Internally the <see cref="M34d"/> is tarnsformed
        /// to a <see cref="M44d"/> to delete the row and column.
        /// </summary>
        /// <param name="deleted_row">Row to delete.</param>
        /// <param name="deleted_column">Column to delete.</param>
        /// <returns>A <see cref="M33d"/>.</returns>
        public M33d Minor(int deleted_row, int deleted_column)
        {
            M44d temp = (M44d)this;

            M33d result      = new M33d();
            int  checked_row = 0;

            for (int actual_row = 0; actual_row < 4; actual_row++)
            {
                int checked_column = 0;

                if (actual_row != deleted_row)
                {
                    for (int actual_column = 0; actual_column < 4; actual_column++)
                    {
                        if (actual_column != deleted_column)
                        {
                            result[checked_row, checked_column] = temp[actual_row, actual_column];
                            checked_column++;
                        }
                    }
                    checked_row++;
                }
            }

            return(result);
        }
Exemple #3
0
        public static Trafo3d FromNormalFrame(V3d origin, V3d normal)
        {
            M44d forward, backward;

            M44d.NormalFrame(origin, normal, out forward, out backward);
            return(new Trafo3d(forward, backward));
        }
Exemple #4
0
        /// <summary>
        /// 2D plane space to 3D world space.
        /// Plane space is defined by a normal-frame from Point and Normal of the plane.
        /// </summary>
        public static M44d GetPlaneToWorld(this Plane3d self)
        {
            M44d local2global, _;

            M44d.NormalFrame(self.Point, self.Normal, out local2global, out _);
            return(local2global);
        }
Exemple #5
0
        /// <summary>
        /// 3D world space to 2D plane space.
        /// Plane space is defined by a normal-frame from Point and Normal of the plane.
        /// </summary>
        public static M44d GetWorldToPlane(this Plane3d self)
        {
            M44d _, global2local;

            M44d.NormalFrame(self.Point, self.Normal, out _, out global2local);
            return(global2local);
        }
Exemple #6
0
 public static void TransformPosArray(this M44d mat, V3d[] points)
 {
     for (int i = 0; i < points.Length; i++)
     {
         points[i] = mat.TransformPos(points[i]);
     }
 }
Exemple #7
0
        public Similarity3d(M44d m, double epsilon = (double)0.00001)
        {
            if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
            {
                throw new ArgumentException("Matrix contains perspective components.");
            }
            if (m.M33.IsTiny(epsilon))
            {
                throw new ArgumentException("Matrix is not homogeneous.");
            }
            m /= m.M33; //normalize it
            var m33 = (M33d)m;
            var s0  = m33.C0.Norm2;
            var s1  = m33.C1.Norm2;
            var s2  = m33.C2.Norm2;
            var s   = (s0 * s1 * s2).Pow((double)1.0 / 3); //geometric mean of scale

            if (!((s0 / s - 1).IsTiny(epsilon) && (s1 / s - 1).IsTiny(epsilon) && (s2 / s - 1).IsTiny(epsilon)))
            {
                throw new ArgumentException("Matrix features non-uniform scaling");
            }
            m33  /= s;
            Scale = s;
            EuclideanTransformation = new Euclidean3d(m33, m.C3.XYZ);
        }
        /// <summary>
        /// Returns the handedness of the given transformation matrix that is assumed to be row-major.
        /// A right-handed coodinate system is given when
        /// (X cross Y) dot Z is positive,
        /// otherwise left-handed.
        /// </summary>
        public static CoordinateSystem.Handedness Handedness(this M44d mat)
        {
            var x = mat.R0.XYZ;
            var y = mat.R1.XYZ;
            var z = mat.R2.XYZ;

            return(x.Cross(y).Dot(z) > 0 ? CoordinateSystem.Handedness.Right : CoordinateSystem.Handedness.Left);
        }
Exemple #9
0
        public static Trafo3d Parse(string s)
        {
            var x = s.NestedBracketSplitLevelOne().ToArray();

            return(new Trafo3d(
                       M44d.Parse(x[0].ToString()),
                       M44d.Parse(x[1].ToString())
                       ));
        }
 public static M44d Add(M34d a, M44d b)
 {
     return(new M44d(
                a.M00 + b.M00, a.M01 + b.M01, a.M02 + b.M02, a.M03 + b.M03,
                a.M10 + b.M10, a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13,
                a.M20 + b.M20, a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23,
                b.M30, b.M31, b.M32, 1 + b.M33
                ));
 }
 public static M44d Subtract(M44d a, M34d b)
 {
     return(new M44d(
                a.M00 - b.M00, a.M01 - b.M01, a.M02 - b.M02, a.M03 - b.M03,
                a.M10 - b.M10, a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13,
                a.M20 - b.M20, a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23,
                a.M30, a.M31, a.M32, a.M33 - 1
                ));
 }
Exemple #12
0
        //public static Quadric operator -(Quadric lhs, Quadric rhs)
        //{
        //    Quadric result = new Quadric();

        //    result.ErrorQuadric = lhs.ErrorQuadric - rhs.ErrorQuadric;

        //    result.ErrorHeuristic = Quadric.ToHeuristic(result.ErrorQuadric);

        //    return result;
        //}

        #endregion

        #region Static Methods

        static M44d ToHeuristic(M44d quadric)
        {
            var result = new M44d();

            result    = quadric;
            result.R3 = new V4d(0, 0, 0, 1);

            return(result);
        }
Exemple #13
0
        public void CreateHeuristic()
        {
            if (m_errorQuadric == M44d.Zero)
            {
                throw new InvalidOperationException("Must call CreateQuadric(...) first");
            }

            ErrorHeuristic = ToHeuristic(ErrorQuadric);
        }
Exemple #14
0
        /// <summary>
        /// Computes from a <see cref="V3d"/> point (origin) and
        /// a <see cref="V3d"/> normal the transformation matrix
        /// and its inverse.
        /// </summary>
        /// <param name="origin">The point which will become the new origin.</param>
        /// <param name="normal">The normal vector of the new ground plane.</param>
        /// <param name="local2global">A <see cref="M44d"/>The trafo from local to global system.</param>
        /// <param name="global2local">A <see cref="M44d"/>The trafofrom global to local system.</param>
        public static void NormalFrame(V3d origin, V3d normal,
                                       out M44d local2global, out M44d global2local
                                       )
        {
            V3d    min;
            double x = Fun.Abs(normal.X);
            double y = Fun.Abs(normal.Y);
            double z = Fun.Abs(normal.Z);

            if (x < y)
            {
                if (x < z)
                {
                    min = V3d.XAxis;
                }
                else
                {
                    min = V3d.ZAxis;
                }
            }
            else
            {
                if (y < z)
                {
                    min = V3d.YAxis;
                }
                else
                {
                    min = V3d.ZAxis;
                }
            }

            V3d xVec = Vec.Cross(normal, min);

            xVec.Normalize(); // this is now guaranteed to be normal to the input normal
            V3d yVec = Vec.Cross(normal, xVec);

            yVec.Normalize();
            V3d zVec = normal;

            zVec.Normalize();

            local2global = new M44d(xVec.X, yVec.X, zVec.X, origin.X,
                                    xVec.Y, yVec.Y, zVec.Y, origin.Y,
                                    xVec.Z, yVec.Z, zVec.Z, origin.Z,
                                    0, 0, 0, 1);

            M44d mat = new M44d(xVec.X, xVec.Y, xVec.Z, 0,
                                yVec.X, yVec.Y, yVec.Z, 0,
                                zVec.X, zVec.Y, zVec.Z, 0,
                                0, 0, 0, 1);

            var shift = M44d.Translation(-origin);

            global2local = mat * shift;
        }
        public static M44d Enlarge(M33d m)
        {
            M44d enlarged = new M44d(
                m.M00, m.M01, m.M02, 0,
                m.M10, m.M11, m.M12, 0,
                m.M20, m.M21, m.M22, 0,
                0, 0, 0, 1.0f);

            return(enlarged);
        }
Exemple #16
0
        /// <summary>
        /// Returns the trafo that transforms from the coordinate system
        /// specified by the basis into the world coordinate system.
        /// </summary>
        public static Trafo3d FromBasis(V3d xAxis, V3d yAxis, V3d zAxis, V3d orign)
        {
            var mat = new M44d(
                xAxis.X, yAxis.X, zAxis.X, orign.X,
                xAxis.Y, yAxis.Y, zAxis.Y, orign.Y,
                xAxis.Z, yAxis.Z, zAxis.Z, orign.Z,
                0, 0, 0, 1);

            return(new Trafo3d(mat, mat.Inverse));
        }
Exemple #17
0
        public static V3d[] TransformedDirArray(this M44d mat, V3d[] directions)
        {
            var result = new V3d[directions.Length];

            for (int i = 0; i < directions.Length; i++)
            {
                result[i] = mat.TransformDir(directions[i]);
            }
            return(result);
        }
Exemple #18
0
        public static V3d[] TransformedPosArray(this M44d mat, ICollection <V3d> points)
        {
            var result = new V3d[points.Count];
            int i      = 0;

            foreach (var p in points)
            {
                result[i++] = mat.TransformPos(p);
            }
            return(result);
        }
Exemple #19
0
 /// <summary>
 /// Creates a rigid transformation from a matrix <paramref name="m"/>.
 /// </summary>
 public Euclidean3d(M44d m, double epsilon = 1e-12)
     : this(((M33d)m) / m.M33, m.C3.XYZ / m.M33, epsilon)
 {
     if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
     {
         throw new ArgumentException("Matrix contains perspective components.");
     }
     if (m.M33.IsTiny(epsilon))
     {
         throw new ArgumentException("Matrix is not homogeneous.");
     }
 }
Exemple #20
0
        /// <summary>
        /// Copies from the position array indexed by a backward map into
        /// a target array, starting at the supplied offset, thereby
        /// transforming all positions using the supplied matrix.
        /// </summary>
        /// <returns>target array</returns>
        public static V3f[] BackwardIndexedTransformPosAndCopyTo(
            this V3f[] source,
            V3f[] target,
            int[] backwardMap,
            int offset,
            M44d m44d)
        {
            var count = backwardMap.Length;

            for (int i = 0; i < count; i++)
            {
                target[i + offset] = (V3f)m44d.TransformPos((V3d)source[backwardMap[i]]);
            }
            return(target);
        }
        public static List <int[]> ComputeNonConcaveSubPolygons(
            this Polygon3d polygon, double absoluteEpsilon)
        {
            V3d    normal = polygon.ComputeDoubleAreaNormal();
            double len2   = normal.LengthSquared;

            if (len2 < absoluteEpsilon * absoluteEpsilon)
            {
                return(new int[polygon.PointCount].SetByIndex(i => i).IntoList());
            }

            M44d.NormalFrame(V3d.Zero, normal * (1.0 / Fun.Sqrt(len2)), out M44d local2global, out M44d global2local);
            var polygon2d = polygon.ToPolygon2d(p => global2local.TransformPos(p).XY);

            return(polygon2d.ComputeNonConcaveSubPolygons(absoluteEpsilon));
        }
Exemple #22
0
        /// <summary>
        /// Builds a hull from the given view-projection transformation (left, right, bottom, top, near, far).
        /// The view volume is assumed to be [-1, -1, -1] [1, 1, 1].
        /// The normals of the hull planes point to the outside and are normalized.
        /// A point inside the visual hull will has negative height to all planes.
        /// </summary>
        public static Hull3d GetVisualHull(this M44d viewProj)
        {
            var r0 = viewProj.R0;
            var r1 = viewProj.R1;
            var r2 = viewProj.R2;
            var r3 = viewProj.R3;

            return(new Hull3d(new[]
            {
                new Plane3d(-(r3 + r0)).Normalized, // left
                new Plane3d(-(r3 - r0)).Normalized, // right
                new Plane3d(-(r3 + r1)).Normalized, // bottom
                new Plane3d(-(r3 - r1)).Normalized, // top
                new Plane3d(-(r3 + r2)).Normalized, // near
                new Plane3d(-(r3 - r2)).Normalized, // far
            }));
        }
Exemple #23
0
 /// <summary>
 /// Computes a Coordiante Frame Transformation (Basis) from current CS into
 /// the (X, Y, Z)-System at a given Origin.
 /// Note: you can use it, to transform from RH to LH and vice-versa, all depending
 /// how you will specifie your new basis-vectors.
 /// </summary>
 /// <param name="xVec">New X Vector</param>
 /// <param name="yVec">New Y Vector</param>
 /// <param name="zVec">New Z vector</param>
 /// <param name="oVec">New Origin.</param>
 /// <param name="viewTrafo"></param>
 /// <param name="viewTrafoInverse"></param>
 public static void CoordinateFrameTransform(V3d xVec, V3d yVec, V3d zVec, V3d oVec,
                                             out M44d viewTrafo, out M44d viewTrafoInverse)
 {
     oVec      = -oVec;
     viewTrafo = new M44d(
         xVec.X, xVec.Y, xVec.Z, xVec.X * oVec.X + xVec.Y * oVec.Y + xVec.Z * oVec.Z,
         yVec.X, yVec.Y, yVec.Z, yVec.X * oVec.X + yVec.Y * oVec.Y + yVec.Z * oVec.Z,
         zVec.X, zVec.Y, zVec.Z, zVec.X * oVec.X + zVec.Y * oVec.Y + zVec.Z * oVec.Z,
         0, 0, 0, 1
         );
     viewTrafoInverse = new M44d(
         xVec.X, yVec.X, zVec.X, -oVec.X,
         xVec.Y, yVec.Y, zVec.Y, -oVec.Y,
         xVec.Z, yVec.Z, zVec.Z, -oVec.Z,
         0, 0, 0, 1
         );
 }
Exemple #24
0
        public static List <int[]> ComputeNonConcaveSubPolygons(
            this V3d[] vertexArray, int polyCount,
            V3d[] normalArray, int[] firstIndices, int[] vertexIndices,
            List <int> faceBackMap,
            double absoluteEpsilon)
        {
            var polyList = new List <int[]>();
            var eps2     = absoluteEpsilon * absoluteEpsilon;

            for (int fvi = 0, fi = 0; fi < polyCount; fi++)
            {
                int fve = firstIndices[fi + 1], fvc = fve - fvi;
                var n  = normalArray[fi];
                var l2 = n.LengthSquared;
                if (l2 < eps2)
                {
                    polyList.Add(new int[fvc].SetByIndex(i => vertexIndices[fvi + i]));
                    if (faceBackMap != null)
                    {
                        faceBackMap.Add(fi);
                    }
                    fvi = fve;
                    continue;
                }
                M44d local2global, global2local;
                M44d.NormalFrame(V3d.Zero, n, out local2global, out global2local);
                var polygon = new Polygon2d(fvc,
                                            i => global2local.TransformPos(vertexArray[vertexIndices[fvi + i]]).XY);
                var subPolyList = polygon.ComputeNonConcaveSubPolygons(absoluteEpsilon);
                foreach (var poly in subPolyList)
                {
                    polyList.Add(poly.Map(i => fvi + i));
                }
                if (faceBackMap != null)
                {
                    for (int i = 0; i < subPolyList.Count; i++)
                    {
                        faceBackMap.Add(fi);
                    }
                }
                fvi = fve;
            }

            return(polyList);
        }
        public static M44d Multiply(Rot2d rot, M44d mat)
        {
            double a = (double)System.Math.Cos(rot.Angle);
            double b = (double)System.Math.Sin(rot.Angle);

            return(new M44d(a * mat.M00 +
                            b * mat.M10,

                            a * mat.M01 +
                            b * mat.M11,

                            a * mat.M02 +
                            b * mat.M12,

                            a * mat.M03 +
                            b * mat.M13,

                            -b * mat.M00 +
                            a * mat.M10,

                            -b * mat.M01 +
                            a * mat.M11,

                            -b * mat.M02 +
                            a * mat.M12,

                            -b * mat.M03 +
                            a * mat.M13,

                            mat.M20,

                            mat.M21,

                            mat.M22,

                            mat.M23,

                            mat.M30,

                            mat.M31,

                            mat.M32,

                            mat.M33));
        }
Exemple #26
0
        /// <summary>
        ///  Provides perspective projection matrix in terms of the vertical field of view angle a and the aspect ratio r.
        /// </summary>
        public static M44d PerspectiveProjectionTransformRH(double a, double r, double n, double f)
        {
            //F / r     0      0      0
            //  0       F      0      0
            //  0       0      A      B
            //  0       0      -1     0
            double F = 1 / Fun.Tan(a / 2);
            double A = f / (n - f);
            double B = f * n / (n - f);

            M44d P = new M44d(
                F / r, 0, 0, 0,
                0, F, 0, 0,
                0, 0, A, B,
                0, 0, -1, 0);

            return(P);
        }
Exemple #27
0
        /// <summary>
        ///  Provides perspective projection matrix.
        ///  The parameters describe the dimensions of the view volume.
        /// </summary>
        public static M44d PerspectiveProjectionTransformRH(V2d size, double n, double f)
        {
            double w = size.X;
            double h = size.Y;
            // Fx      0      0      0
            //  0      Fy     0      0
            //  0      0      A      B
            //  0      0      -1     0
            double Fx = 2 * n / w;
            double Fy = 2 * n / h;
            double A  = f / (n - f);
            double B  = n * f / (n - f);
            M44d   P  = new M44d(
                Fx, 0, 0, 0,
                0, Fy, 0, 0,
                0, 0, A, B,
                0, 0, -1, 0);

            return(P);
        }
Exemple #28
0
        public static Array BackwardIndexedTransformPosAndCopyTo(
            this Array source, Array target,
            int[] backwardMap, int offset,
            M44d m44d)
        {
            Type type = source.GetType();

            if (type == typeof(V3f[]))
            {
                return(BackwardIndexedTransformPosAndCopyTo((V3f[])source, (V3f[])target,
                                                            backwardMap, offset, m44d));
            }
            if (type == typeof(V3d[]))
            {
                return(BackwardIndexedTransformPosAndCopyTo((V3d[])source, (V3d[])target,
                                                            backwardMap, offset, m44d));
            }

            throw new InvalidOperationException();
        }
Exemple #29
0
        /// <summary>
        /// Builds a customized, left-handed perspective Off-Center projection matrix.
        /// </summary>
        public static M44d PerspectiveProjectionTransformLH(double l, double r, double t, double b, double n, double f)
        {
            //  Fx     0      0     0
            //  0      Fy     0     0
            //  Sx     Sy     A     1
            //  0      0      B     0
            double Fx = 2 * n / (r - l);
            double Fy = 2 * n / (t - b);
            double Sx = (l + r) / (l - r);
            double Sy = (t + b) / (b - t);
            double A  = f / (f - n);
            double B  = n * f / (n - f);

            M44d P = new M44d(
                Fx, 0, 0, 0,
                0, Fy, 0, 0,
                Sx, Sy, A, 1,
                0, 0, B, 0);

            return(P);
        }
Exemple #30
0
        /// <summary>
        /// Builds a customized, right-handed perspective Off-Center projection matrix.
        /// </summary>
        public static M44d PerspectiveProjectionTransformRH(double l, double r, double t, double b, double n, double f)
        {
            // Fx      0      Sx     0
            //  0      Fy     Sy     0
            //  0      0      A      B
            //  0      0      -1     0
            double Fx = 2 * n / (r - l);
            double Fy = 2 * n / (t - b);
            double Sx = (l + r) / (r - l);
            double Sy = (t + b) / (t - b);
            double A  = f / (n - f);
            double B  = n * f / (n - f);

            M44d P = new M44d(
                Fx, 0, Sx, 0,
                0, Fy, Sy, 0,
                0, 0, A, B,
                0, 0, -1, 0);

            return(P);
        }