Ejemplo n.º 1
0
        static public Plane3 <T> MultiplyByMatrix(Plane3 <T> plane, Matrix4d4 <T> m, T unit)
        {
            Vector3d <T> dir1    = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % plane.normal;
            Numeric <T>  dir1Len = dir1 ^ dir1;

            Vector3d <T> tmp    = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % plane.normal;
            Numeric <T>  tmpLen = tmp ^ tmp;

            if (tmpLen > dir1Len)
            {
                dir1    = tmp;
                dir1Len = tmpLen;
            }

            tmp    = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % plane.normal;
            tmpLen = tmp ^ tmp;

            if (tmpLen > dir1Len)
            {
                dir1 = tmp;
            }

            Vector3d <T> dir2  = dir1 % plane.normal;
            Vector3d <T> point = plane.distance * plane.normal;

            return(new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit));
        }
Ejemplo n.º 2
0
        public Matrix4d4(Matrix4d4 <T> v)
        {
            x = new Numeric <T> [4][];

            for (int i = 0; i < 4; i++)
            {
                x[i] = new Numeric <T> [4];
            }

            x[0][0] = v.x[0][0];
            x[0][1] = v.x[0][1];
            x[0][2] = v.x[0][2];
            x[0][3] = v.x[0][3];
            x[1][0] = v.x[1][0];
            x[1][1] = v.x[1][1];
            x[1][2] = v.x[1][2];
            x[1][3] = v.x[1][3];
            x[2][0] = v.x[2][0];
            x[2][1] = v.x[2][1];
            x[2][2] = v.x[2][2];
            x[2][3] = v.x[2][3];
            x[3][0] = v.x[3][0];
            x[3][1] = v.x[3][1];
            x[3][2] = v.x[3][2];
            x[3][3] = v.x[3][3];
        }
Ejemplo n.º 3
0
        //public IntersectionResult<Vector3d<T>> Intersect(Line3<T> line)
        //{
        //    Numeric<T> d = normal ^ line.Dir;

        //    if (d.Equals(Numeric<T>.Zero()))
        //        return new IntersectionResult<Vector3d<T>>(false);

        //    T t = -((normal ^ line.From) - distance) / d;
        //    Vector3d<T> point = line.GetPoint(t);

        //    return new IntersectionResult<Vector3d<T>>(point, true);
        //}

        //public IntersectionResult<T> IntersectT(Line3<T> line)
        //{
        //    Numeric<T> d = normal ^ line.Dir;

        //    if (d.Equals(Numeric<T>.Zero()))
        //        return new IntersectionResult<T>(false);

        //    T t = -((normal ^ line.From) - distance) / d;

        //    return new IntersectionResult<T>(t, true);
        //}

        public void MultiplyByMatrix(Matrix4d4 <T> m, T unit)
        {
            Vector3d <T> dir1    = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % normal;
            Numeric <T>  dir1Len = dir1 ^ dir1;

            Vector3d <T> tmp    = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % normal;
            Numeric <T>  tmpLen = tmp ^ tmp;

            if (tmpLen > dir1Len)
            {
                dir1    = tmp;
                dir1Len = tmpLen;
            }

            tmp    = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % normal;
            tmpLen = tmp ^ tmp;

            if (tmpLen > dir1Len)
            {
                dir1 = tmp;
            }

            Vector3d <T> dir2  = dir1 % normal;
            Vector3d <T> point = distance * normal;

            this = new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// Matrix multiplication
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static Matrix4d4 <T> operator *(Matrix4d4 <T> v1, Matrix4d4 <T> v2)
        {
            Matrix4d4 <T> tmp = new Matrix4d4 <T>(Numeric <T> .Zero());

            Matrix4d4 <T> .Multiply(v1, v2, ref tmp);

            return(tmp);
        }
Ejemplo n.º 5
0
        public static Matrix4d4 <T> Identity(T unit)
        {
            Matrix4d4 <T> m = new Matrix4d4 <T>(Numeric <T> .Zero());

            m.MakeIdentity(unit);

            return(m);
        }
Ejemplo n.º 6
0
 public override bool Equals(object obj)
 {
     if (obj != null && obj is Matrix4d4 <T> )
     {
         Matrix4d4 <T> other = (Matrix4d4 <T>)obj;
         return(Equals(ref this, ref other));
     }
     return(base.Equals(obj));
 }
Ejemplo n.º 7
0
        public Matrix4d4 <T> Shear(Shear6 <T> h)
        {
            Matrix4d4 <T> P = new Matrix4d4 <T>(this);

            for (int i = 0; i < 4; i++)
            {
                x[0][i] = P[0][i] + h.YX * P[1][i] + h.ZX * P[2][i];
                x[1][i] = h.XY * P[0][i] + P[1][i] + h.ZY * P[2][i];
                x[2][i] = h.XZ * P[0][i] + h.YZ * P[1][i] + P[2][i];
            }

            return(this);
        }
Ejemplo n.º 8
0
        public Matrix4d4 <V> Select <V>(Func <T, V> transf) where V : IEquatable <V>
        {
            Matrix4d4 <V> transfMatrix = new Matrix4d4 <V>(Numeric <V> .Zero());

            for (int i = 0; i < 4; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    transfMatrix[i][j] = transf(x[i][j]);
                }
            }

            return(transfMatrix);
        }
Ejemplo n.º 9
0
        /// <summary>
        /// Set matrix to rotation by angle
        /// </summary>
        /// <param name="angle">angle of rotation in radians</param>
        /// <returns></returns>
        public Matrix4d4 <T> Rotate(Vector3d <T> angle, T unit)
        {
            Numeric <T> cosRZ, sinRZ, cosRY, sinRY, cosRX, sinRX;
            Numeric <T> m00, m01, m02;
            Numeric <T> m10, m11, m12;
            Numeric <T> m20, m21, m22;

            T rx = angle.X;
            T ry = angle.Y;
            T rz = angle.Z;

            cosRZ = (GenericMath.Cos(rz, unit));
            cosRY = (GenericMath.Cos(ry, unit));
            cosRX = (GenericMath.Cos(rx, unit));

            sinRZ = (GenericMath.Sin(rz, unit));
            sinRY = (GenericMath.Sin(ry, unit));
            sinRX = (GenericMath.Sin(rx, unit));

            m00 = cosRZ * cosRY;
            m01 = sinRZ * cosRY;
            m02 = -sinRY;
            m10 = -sinRZ * cosRX + cosRZ * sinRY * sinRX;
            m11 = cosRZ * cosRX + sinRZ * sinRY * sinRX;
            m12 = cosRY * sinRX;
            m20 = -sinRZ * -sinRX + cosRZ * sinRY * cosRX;
            m21 = cosRZ * -sinRX + sinRZ * sinRY * cosRX;
            m22 = cosRY * cosRX;

            Matrix4d4 <T> P = new Matrix4d4 <T>(this);

            x[0][0] = P[0][0] * m00 + P[1][0] * m01 + P[2][0] * m02;
            x[0][1] = P[0][1] * m00 + P[1][1] * m01 + P[2][1] * m02;
            x[0][2] = P[0][2] * m00 + P[1][2] * m01 + P[2][2] * m02;
            x[0][3] = P[0][3] * m00 + P[1][3] * m01 + P[2][3] * m02;

            x[1][0] = P[0][0] * m10 + P[1][0] * m11 + P[2][0] * m12;
            x[1][1] = P[0][1] * m10 + P[1][1] * m11 + P[2][1] * m12;
            x[1][2] = P[0][2] * m10 + P[1][2] * m11 + P[2][2] * m12;
            x[1][3] = P[0][3] * m10 + P[1][3] * m11 + P[2][3] * m12;

            x[2][0] = P[0][0] * m20 + P[1][0] * m21 + P[2][0] * m22;
            x[2][1] = P[0][1] * m20 + P[1][1] * m21 + P[2][1] * m22;
            x[2][2] = P[0][2] * m20 + P[1][2] * m21 + P[2][2] * m22;
            x[2][3] = P[0][3] * m20 + P[1][3] * m21 + P[2][3] * m22;

            return(this);
        }
Ejemplo n.º 10
0
        /// <summary>
        /// Matrix multiplication
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        static public void Multiply(Matrix4d4 <T> a, Matrix4d4 <T> b, ref Matrix4d4 <T> c)
        {
            T a0, a1, a2, a3;

            a0 = a[0][0];
            a1 = a[0][1];
            a2 = a[0][2];
            a3 = a[0][3];

            c[0][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0];
            c[0][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1];
            c[0][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2];
            c[0][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3];

            a0 = a[1][0];
            a1 = a[1][1];
            a2 = a[1][2];
            a3 = a[1][3];

            c[1][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0];
            c[1][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1];
            c[1][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2];
            c[1][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3];

            a0 = a[2][0];
            a1 = a[2][1];
            a2 = a[2][2];
            a3 = a[2][3];

            c[2][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0];
            c[2][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1];
            c[2][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2];
            c[2][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3];

            a0 = a[3][0];
            a1 = a[3][1];
            a2 = a[3][2];
            a3 = a[3][3];

            c[3][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0];
            c[3][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1];
            c[3][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2];
            c[3][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3];
        }
Ejemplo n.º 11
0
 private static bool Equals(ref Matrix4d4 <T> v1, ref Matrix4d4 <T> v2)
 {
     return(EqualityComparer <T> .Default.Equals(v1.x[0][0], v2.x[0][0]) &&
            EqualityComparer <T> .Default.Equals(v1.x[0][1], v2.x[0][1]) &&
            EqualityComparer <T> .Default.Equals(v1.x[0][2], v2.x[0][2]) &&
            EqualityComparer <T> .Default.Equals(v1.x[0][3], v2.x[0][3]) &&
            EqualityComparer <T> .Default.Equals(v1.x[1][0], v2.x[1][0]) &&
            EqualityComparer <T> .Default.Equals(v1.x[1][1], v2.x[1][1]) &&
            EqualityComparer <T> .Default.Equals(v1.x[1][2], v2.x[1][2]) &&
            EqualityComparer <T> .Default.Equals(v1.x[1][3], v2.x[1][3]) &&
            EqualityComparer <T> .Default.Equals(v1.x[2][0], v2.x[2][0]) &&
            EqualityComparer <T> .Default.Equals(v1.x[2][1], v2.x[2][1]) &&
            EqualityComparer <T> .Default.Equals(v1.x[2][2], v2.x[2][2]) &&
            EqualityComparer <T> .Default.Equals(v1.x[2][3], v2.x[2][3]) &&
            EqualityComparer <T> .Default.Equals(v1.x[3][0], v2.x[3][0]) &&
            EqualityComparer <T> .Default.Equals(v1.x[3][1], v2.x[3][1]) &&
            EqualityComparer <T> .Default.Equals(v1.x[3][2], v2.x[3][2]) &&
            EqualityComparer <T> .Default.Equals(v1.x[3][3], v2.x[3][3]));
 }
Ejemplo n.º 12
0
        Matrix4d4 <T> SetTheMatrix(Matrix4d4 <T> v)
        {
            x[0][0] = v.x[0][0];
            x[0][1] = v.x[0][1];
            x[0][2] = v.x[0][2];
            x[0][3] = v.x[0][3];
            x[1][0] = v.x[1][0];
            x[1][1] = v.x[1][1];
            x[1][2] = v.x[1][2];
            x[1][3] = v.x[1][3];
            x[2][0] = v.x[2][0];
            x[2][1] = v.x[2][1];
            x[2][2] = v.x[2][2];
            x[2][3] = v.x[2][3];
            x[3][0] = v.x[3][0];
            x[3][1] = v.x[3][1];
            x[3][2] = v.x[3][2];
            x[3][3] = v.x[3][3];

            return(this);
        }
Ejemplo n.º 13
0
        /// <summary>
        /// Transpose the matrix
        /// </summary>
        /// <returns></returns>
        public Matrix4d4 <T> Transpose()
        {
            Matrix4d4 <T> tmp = new Matrix4d4 <T>(x[0][0],
                                                  x[1][0],
                                                  x[2][0],
                                                  x[3][0],
                                                  x[0][1],
                                                  x[1][1],
                                                  x[2][1],
                                                  x[3][1],
                                                  x[0][2],
                                                  x[1][2],
                                                  x[2][2],
                                                  x[3][2],
                                                  x[0][3],
                                                  x[1][3],
                                                  x[2][3],
                                                  x[3][3]);

            this = tmp;

            return(this);
        }
Ejemplo n.º 14
0
        /// <summary>
        /// Gauss–Jordan elimination
        /// </summary>
        /// <exception cref="MatrixNotInvertibleException">MatrixNotInvertibleException</exception>
        /// <returns></returns>
        public Matrix4d4 <T> Inverse(T unit)
        {
            int           i, j, k;
            Matrix4d4 <T> s = Matrix4d4 <T> .Identity(unit);

            Matrix4d4 <T> t = new Matrix4d4 <T>(this);

            // Forward elimination

            for (i = 0; i < 3; i++)
            {
                int pivot = i;

                Numeric <T> pivotsize = (t[i][i]);

                if (pivotsize < Numeric <T> .Zero())
                {
                    pivotsize = -pivotsize;
                }

                for (j = i + 1; j < 4; j++)
                {
                    Numeric <T> tmp = (t[j][i]);

                    if (tmp < Numeric <T> .Zero())
                    {
                        tmp = -tmp;
                    }

                    if (tmp > pivotsize)
                    {
                        pivot     = j;
                        pivotsize = tmp;
                    }
                }

                if (pivotsize.Equals(Numeric <T> .Zero()))
                {
                    throw new Exception("Cannot invert singular matrix.");
                }

                if (pivot != i)
                {
                    for (j = 0; j < 4; j++)
                    {
                        T tmp;

                        tmp         = t[i][j];
                        t[i][j]     = t[pivot][j];
                        t[pivot][j] = tmp;

                        tmp         = s[i][j];
                        s[i][j]     = s[pivot][j];
                        s[pivot][j] = tmp;
                    }
                }

                for (j = i + 1; j < 4; j++)
                {
                    T f = t[j][i] / t[i][i];

                    for (k = 0; k < 4; k++)
                    {
                        t[j][k] -= f * t[i][k];
                        s[j][k] -= f * s[i][k];
                    }
                }
            }

            // Backward substitution

            for (i = 3; i >= 0; --i)
            {
                Numeric <T> f;

                if ((f = t[i][i]).Equals(Numeric <T> .Zero()))
                {
                    throw new Exception("Cannot invert singular matrix.");
                }

                for (j = 0; j < 4; j++)
                {
                    t[i][j] /= f;
                    s[i][j] /= f;
                }

                for (j = 0; j < i; j++)
                {
                    f = t[j][i];

                    for (k = 0; k < 4; k++)
                    {
                        t[j][k] -= f * t[i][k];
                        s[j][k] -= f * s[i][k];
                    }
                }
            }

            return(s);
        }
Ejemplo n.º 15
0
 public bool Equals(Matrix4d4 <T> other)
 {
     return(Equals(this, other));
 }
Ejemplo n.º 16
0
        //public Vector3d<T> ClosestPointTo(Line3<T> line, Func<double, T> fromFloat = null, Func<T, double> magnitude = null)
        //{
        //    if (fromFloat == null)
        //        fromFloat = Numeric<T>.FromFloat;

        //    if (magnitude == null)
        //        magnitude = Numeric<T>.ToFloat;

        //    // Assumes the lines are normalized

        //    Vector3d<T> posLpos = fromFloat - line.from;
        //    Numeric<T> c = dir ^ posLpos;
        //    Numeric<T> a = line.dir ^ dir;
        //    Numeric<T> f = line.dir ^ posLpos;
        //    Numeric<T> num = c - a * f;

        //    Numeric<T> denom = a * a - fromFloat(1);

        //    Numeric<T> absDenom = ((denom >= Numeric<T>.Zero()) ? denom : -denom);

        //    if (absDenom < fromdouble(1))
        //    {
        //        Numeric<T> absNum = ((num >= Numeric<T>.Zero()) ? num : -num);

        //        if (magnitude(absNum) >= magnitude(absDenom) * double.MaxValue)
        //            return fromFloat;
        //    }

        //    return fromFloat + dir * (num / denom);
        //}

        public void Multiply(Matrix4d4 <T> m, T unit)
        {
            this = new Line3 <T>(from * m, (from + dir) * m, unit);
        }