Ejemplo n.º 1
0
 /// <summary>Sets the value of this matrix to that of the Matrix3f provided.</summary>
 /// <remarks>Sets the value of this matrix to that of the Matrix3f provided.</remarks>
 /// <param name="m1">the matrix</param>
 public void Set(Matrix3f m1)
 {
     int i;
     int j;
     if (nCol < 3 || nRow < 3)
     {
         // expand matrix if too small
         nCol = 3;
         nRow = 3;
         //values = new double[nRow][nCol];
         values = new double[][] { new double[3], new double[3], new double[3] };
     }
     values[0][0] = m1.m00;
     values[0][1] = m1.m01;
     values[0][2] = m1.m02;
     values[1][0] = m1.m10;
     values[1][1] = m1.m11;
     values[1][2] = m1.m12;
     values[2][0] = m1.m20;
     values[2][1] = m1.m21;
     values[2][2] = m1.m22;
     for (i = 3; i < nRow; i++)
     {
         // pad rest or matrix with zeros
         for (j = 3; j < nCol; j++)
         {
             values[i][j] = 0.0;
         }
     }
 }
Ejemplo n.º 2
0
 /// <summary>
 /// Multiplies each element of matrix m1 by a scalar and places
 /// the result into this.
 /// </summary>
 /// <remarks>
 /// Multiplies each element of matrix m1 by a scalar and places
 /// the result into this.  Matrix m1 is not modified.
 /// </remarks>
 /// <param name="scalar">the scalar multiplier</param>
 /// <param name="m1">the original matrix</param>
 public void Mul(float scalar, Matrix3f m1)
 {
     this.m00 = scalar * m1.m00;
     this.m01 = scalar * m1.m01;
     this.m02 = scalar * m1.m02;
     this.m10 = scalar * m1.m10;
     this.m11 = scalar * m1.m11;
     this.m12 = scalar * m1.m12;
     this.m20 = scalar * m1.m20;
     this.m21 = scalar * m1.m21;
     this.m22 = scalar * m1.m22;
 }
Ejemplo n.º 3
0
 /// <summary>
 /// Sets the value of this matrix to the result of multiplying
 /// the two argument matrices together.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the result of multiplying
 /// the two argument matrices together.
 /// </remarks>
 /// <param name="m1">the first matrix</param>
 /// <param name="m2">the second matrix</param>
 public void Mul(Matrix3f m1, Matrix3f m2)
 {
     if (this != m1 && this != m2)
     {
         this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
         this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
         this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
         this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
         this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
         this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
         this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
         this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
         this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
     }
     else
     {
         float m00;
         float m01;
         float m02;
         float m10;
         float m11;
         float m12;
         float m20;
         float m21;
         float m22;
         m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
         m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
         m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
         m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
         m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
         m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
         m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
         m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
         m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
         this.m00 = m00;
         this.m01 = m01;
         this.m02 = m02;
         this.m10 = m10;
         this.m11 = m11;
         this.m12 = m12;
         this.m20 = m20;
         this.m21 = m21;
         this.m22 = m22;
     }
 }
Ejemplo n.º 4
0
 /// <summary>General invert routine.</summary>
 /// <remarks>
 /// General invert routine.  Inverts m1 and places the result in "this".
 /// Note that this routine handles both the "this" version and the
 /// non-"this" version.
 /// Also note that since this routine is slow anyway, we won't worry
 /// about allocating a little bit of garbage.
 /// </remarks>
 private void InvertGeneral(Matrix3f m1)
 {
     double[] temp = new double[9];
     double[] result = new double[9];
     int[] row_perm = new int[3];
     int i;
     int r;
     int c;
     // Use LU decomposition and backsubstitution code specifically
     // for floating-point 3x3 matrices.
     // Copy source matrix to t1tmp
     temp[0] = (double)m1.m00;
     temp[1] = (double)m1.m01;
     temp[2] = (double)m1.m02;
     temp[3] = (double)m1.m10;
     temp[4] = (double)m1.m11;
     temp[5] = (double)m1.m12;
     temp[6] = (double)m1.m20;
     temp[7] = (double)m1.m21;
     temp[8] = (double)m1.m22;
     // Calculate LU decomposition: Is the matrix singular?
     if (!LuDecomposition(temp, row_perm))
     {
         // Matrix has no inverse
         throw new SingularMatrixException("cannot invert matrix");
     }
     // Perform back substitution on the identity matrix
     for (i = 0; i < 9; i++)
     {
         result[i] = 0.0;
     }
     result[0] = 1.0;
     result[4] = 1.0;
     result[8] = 1.0;
     LuBacksubstitution(temp, row_perm, result);
     this.m00 = (float)result[0];
     this.m01 = (float)result[1];
     this.m02 = (float)result[2];
     this.m10 = (float)result[3];
     this.m11 = (float)result[4];
     this.m12 = (float)result[5];
     this.m20 = (float)result[6];
     this.m21 = (float)result[7];
     this.m22 = (float)result[8];
 }
Ejemplo n.º 5
0
 /// <summary>
 /// Returns true if all of the data members of Matrix3f m1 are
 /// equal to the corresponding data members in this Matrix3f.
 /// </summary>
 /// <remarks>
 /// Returns true if all of the data members of Matrix3f m1 are
 /// equal to the corresponding data members in this Matrix3f.
 /// </remarks>
 /// <param name="m1">the matrix with which the comparison is made</param>
 /// <returns>true or false</returns>
 public virtual bool Equals(Matrix3f m1)
 {
     try
     {
         return (this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02 && this.m10
              == m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12 && this.m20 == m1.m20 &&
             this.m21 == m1.m21 && this.m22 == m1.m22);
     }
     catch (ArgumentNullException)
     {
         return false;
     }
 }
Ejemplo n.º 6
0
 /// <summary>
 /// Sets the value of this matrix to the matrix difference
 /// of itself and matrix m1 (this = this - m1).
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the matrix difference
 /// of itself and matrix m1 (this = this - m1).
 /// </remarks>
 /// <param name="m1">the other matrix</param>
 public void Sub(Matrix3f m1)
 {
     this.m00 -= m1.m00;
     this.m01 -= m1.m01;
     this.m02 -= m1.m02;
     this.m10 -= m1.m10;
     this.m11 -= m1.m11;
     this.m12 -= m1.m12;
     this.m20 -= m1.m20;
     this.m21 -= m1.m21;
     this.m22 -= m1.m22;
 }
Ejemplo n.º 7
0
 /// <summary>Sets the value of this matrix to the transpose of the argument matrix.</summary>
 /// <remarks>Sets the value of this matrix to the transpose of the argument matrix.</remarks>
 /// <param name="m1">the matrix to be transposed</param>
 public void Transpose(Matrix3f m1)
 {
     if (this != m1)
     {
         this.m00 = m1.m00;
         this.m01 = m1.m10;
         this.m02 = m1.m20;
         this.m10 = m1.m01;
         this.m11 = m1.m11;
         this.m12 = m1.m21;
         this.m20 = m1.m02;
         this.m21 = m1.m12;
         this.m22 = m1.m22;
     }
     else
     {
         this.Transpose();
     }
 }
Ejemplo n.º 8
0
 /// <summary>
 /// Sets the rotational component (upper 3x3) of this matrix to the
 /// matrix values in the single precision Matrix3f argument; the other
 /// elements of this matrix are unchanged; a singular value
 /// decomposition is performed on this object's upper 3x3 matrix to
 /// factor out the scale, then this object's upper 3x3 matrix components
 /// are replaced by the passed rotation components,
 /// and then the scale is reapplied to the rotational components.
 /// </summary>
 /// <remarks>
 /// Sets the rotational component (upper 3x3) of this matrix to the
 /// matrix values in the single precision Matrix3f argument; the other
 /// elements of this matrix are unchanged; a singular value
 /// decomposition is performed on this object's upper 3x3 matrix to
 /// factor out the scale, then this object's upper 3x3 matrix components
 /// are replaced by the passed rotation components,
 /// and then the scale is reapplied to the rotational components.
 /// </remarks>
 /// <param name="m1">single precision 3x3 matrix</param>
 public void SetRotation(Matrix3f m1)
 {
     double[] tmp_rot = new double[9];
     // scratch matrix
     double[] tmp_scale = new double[3];
     // scratch matrix
     GetScaleRotate(tmp_scale, tmp_rot);
     m00 = (float)(m1.m00 * tmp_scale[0]);
     m01 = (float)(m1.m01 * tmp_scale[1]);
     m02 = (float)(m1.m02 * tmp_scale[2]);
     m10 = (float)(m1.m10 * tmp_scale[0]);
     m11 = (float)(m1.m11 * tmp_scale[1]);
     m12 = (float)(m1.m12 * tmp_scale[2]);
     m20 = (float)(m1.m20 * tmp_scale[0]);
     m21 = (float)(m1.m21 * tmp_scale[1]);
     m22 = (float)(m1.m22 * tmp_scale[2]);
 }
Ejemplo n.º 9
0
 /// <summary>
 /// Replaces the upper 3x3 matrix values of this matrix with the
 /// values in the matrix m1.
 /// </summary>
 /// <remarks>
 /// Replaces the upper 3x3 matrix values of this matrix with the
 /// values in the matrix m1.
 /// </remarks>
 /// <param name="m1">the matrix that will be the new upper 3x3</param>
 public void SetRotationScale(Matrix3f m1)
 {
     m00 = m1.m00;
     m01 = m1.m01;
     m02 = m1.m02;
     m10 = m1.m10;
     m11 = m1.m11;
     m12 = m1.m12;
     m20 = m1.m20;
     m21 = m1.m21;
     m22 = m1.m22;
 }
Ejemplo n.º 10
0
 /// <summary>
 /// Sets the rotational component (upper 3x3) of this matrix to the
 /// matrix values in the single precision Matrix3f argument; the other
 /// elements of this matrix are initialized as if this were an identity
 /// matrix (i.e., affine matrix with no translational component).
 /// </summary>
 /// <remarks>
 /// Sets the rotational component (upper 3x3) of this matrix to the
 /// matrix values in the single precision Matrix3f argument; the other
 /// elements of this matrix are initialized as if this were an identity
 /// matrix (i.e., affine matrix with no translational component).
 /// </remarks>
 /// <param name="m1">the single-precision 3x3 matrix</param>
 public void Set(Matrix3f m1)
 {
     m00 = m1.m00;
     m01 = m1.m01;
     m02 = m1.m02;
     m03 = 0.0f;
     m10 = m1.m10;
     m11 = m1.m11;
     m12 = m1.m12;
     m13 = 0.0f;
     m20 = m1.m20;
     m21 = m1.m21;
     m22 = m1.m22;
     m23 = 0.0f;
     m30 = 0.0f;
     m31 = 0.0f;
     m32 = 0.0f;
     m33 = 1.0f;
 }
Ejemplo n.º 11
0
 /// <summary>
 /// Constructs and initializes a Matrix4f from the rotation matrix,
 /// translation, and scale values; the scale is applied only to the
 /// rotational components of the matrix (upper 3x3) and not to the
 /// translational components of the matrix.
 /// </summary>
 /// <remarks>
 /// Constructs and initializes a Matrix4f from the rotation matrix,
 /// translation, and scale values; the scale is applied only to the
 /// rotational components of the matrix (upper 3x3) and not to the
 /// translational components of the matrix.
 /// </remarks>
 /// <param name="m1">the rotation matrix representing the rotational components</param>
 /// <param name="t1">the translational components of the matrix</param>
 /// <param name="s">the scale value applied to the rotational components</param>
 public Matrix4f(Matrix3f m1, Vector3f t1, float s)
 {
     this.m00 = m1.m00 * s;
     this.m01 = m1.m01 * s;
     this.m02 = m1.m02 * s;
     this.m03 = t1.x;
     this.m10 = m1.m10 * s;
     this.m11 = m1.m11 * s;
     this.m12 = m1.m12 * s;
     this.m13 = t1.y;
     this.m20 = m1.m20 * s;
     this.m21 = m1.m21 * s;
     this.m22 = m1.m22 * s;
     this.m23 = t1.z;
     this.m30 = 0.0f;
     this.m31 = 0.0f;
     this.m32 = 0.0f;
     this.m33 = 1.0f;
 }
Ejemplo n.º 12
0
 /// <summary>
 /// Sets the value of this axis-angle to the rotational component of
 /// the passed matrix.
 /// </summary>
 /// <remarks>
 /// Sets the value of this axis-angle to the rotational component of
 /// the passed matrix.
 /// If the specified matrix has no rotational component, the value
 /// of this AxisAngle4d is set to an angle of 0 about an axis of (0,1,0).
 /// </remarks>
 /// <param name="m1">the matrix3f</param>
 public void Set(Matrix3f m1)
 {
     x = (float)(m1.m21 - m1.m12);
     y = (float)(m1.m02 - m1.m20);
     z = (float)(m1.m10 - m1.m01);
     double mag = x * x + y * y + z * z;
     if (mag > Eps)
     {
         mag = Math.Sqrt(mag);
         double sin = 0.5 * mag;
         double cos = 0.5 * (m1.m00 + m1.m11 + m1.m22 - 1.0);
         angle = (float)Math.Atan2(sin, cos);
         double invMag = 1.0 / mag;
         x = x * invMag;
         y = y * invMag;
         z = z * invMag;
     }
     else
     {
         x = 0.0f;
         y = 1.0f;
         z = 0.0f;
         angle = 0.0f;
     }
 }
Ejemplo n.º 13
0
 /// <summary>
 /// Sets the value of this axis-angle to the rotational component of
 /// the passed matrix.
 /// </summary>
 /// <remarks>
 /// Sets the value of this axis-angle to the rotational component of
 /// the passed matrix.
 /// If the specified matrix has no rotational component, the value
 /// of this AxisAngle4f is set to an angle of 0 about an axis of (0,1,0).
 /// </remarks>
 /// <param name="m1">the matrix4f</param>
 public void Set(Matrix4f m1)
 {
     Matrix3f m3f = new Matrix3f();
     m1.Get(m3f);
     x = m3f.m21 - m3f.m12;
     y = m3f.m02 - m3f.m20;
     z = m3f.m10 - m3f.m01;
     double mag = x * x + y * y + z * z;
     if (mag > Eps)
     {
         mag = Math.Sqrt(mag);
         double sin = 0.5 * mag;
         double cos = 0.5 * (m3f.m00 + m3f.m11 + m3f.m22 - 1.0);
         angle = (float)Math.Atan2(sin, cos);
         double invMag = 1.0 / mag;
         x = (float)(x * invMag);
         y = (float)(y * invMag);
         z = (float)(z * invMag);
     }
     else
     {
         x = 0.0f;
         y = 1.0f;
         z = 0.0f;
         angle = 0.0f;
     }
 }
Ejemplo n.º 14
0
 /// <summary>
 /// Places the values in the upper 3x3 of this GMatrix into
 /// the matrix m1.
 /// </summary>
 /// <remarks>
 /// Places the values in the upper 3x3 of this GMatrix into
 /// the matrix m1.
 /// </remarks>
 /// <param name="m1">The matrix that will hold the new values</param>
 public void Get(Matrix3f m1)
 {
     if (nRow < 3 || nCol < 3)
     {
         m1.SetZero();
         if (nCol > 0)
         {
             if (nRow > 0)
             {
                 m1.m00 = (float)values[0][0];
                 if (nRow > 1)
                 {
                     m1.m10 = (float)values[1][0];
                     if (nRow > 2)
                     {
                         m1.m20 = (float)values[2][0];
                     }
                 }
             }
             if (nCol > 1)
             {
                 if (nRow > 0)
                 {
                     m1.m01 = (float)values[0][1];
                     if (nRow > 1)
                     {
                         m1.m11 = (float)values[1][1];
                         if (nRow > 2)
                         {
                             m1.m21 = (float)values[2][1];
                         }
                     }
                 }
                 if (nCol > 2)
                 {
                     if (nRow > 0)
                     {
                         m1.m02 = (float)values[0][2];
                         if (nRow > 1)
                         {
                             m1.m12 = (float)values[1][2];
                             if (nRow > 2)
                             {
                                 m1.m22 = (float)values[2][2];
                             }
                         }
                     }
                 }
             }
         }
     }
     else
     {
         m1.m00 = (float)values[0][0];
         m1.m01 = (float)values[0][1];
         m1.m02 = (float)values[0][2];
         m1.m10 = (float)values[1][0];
         m1.m11 = (float)values[1][1];
         m1.m12 = (float)values[1][2];
         m1.m20 = (float)values[2][0];
         m1.m21 = (float)values[2][1];
         m1.m22 = (float)values[2][2];
     }
 }
Ejemplo n.º 15
0
 /// <summary>
 /// Adds a scalar to each component of the matrix m1 and places
 /// the result into this.
 /// </summary>
 /// <remarks>
 /// Adds a scalar to each component of the matrix m1 and places
 /// the result into this.  Matrix m1 is not modified.
 /// </remarks>
 /// <param name="scalar">the scalar adder.</param>
 /// <param name="m1">the original matrix values</param>
 public void Add(float scalar, Matrix3f m1)
 {
     this.m00 = m1.m00 + scalar;
     this.m01 = m1.m01 + scalar;
     this.m02 = m1.m02 + scalar;
     this.m10 = m1.m10 + scalar;
     this.m11 = m1.m11 + scalar;
     this.m12 = m1.m12 + scalar;
     this.m20 = m1.m20 + scalar;
     this.m21 = m1.m21 + scalar;
     this.m22 = m1.m22 + scalar;
 }
Ejemplo n.º 16
0
 /// <summary>
 /// Performs an SVD normalization of this matrix in order to acquire
 /// the normalized rotational component; the values are placed into
 /// the Matrix3f parameter.
 /// </summary>
 /// <remarks>
 /// Performs an SVD normalization of this matrix in order to acquire
 /// the normalized rotational component; the values are placed into
 /// the Matrix3f parameter.
 /// </remarks>
 /// <param name="m1">matrix into which the rotational component is placed</param>
 public void Get(Matrix3f m1)
 {
     double[] tmp_rot = new double[9];
     // scratch matrix
     double[] tmp_scale = new double[3];
     // scratch matrix
     GetScaleRotate(tmp_scale, tmp_rot);
     m1.m00 = (float)tmp_rot[0];
     m1.m01 = (float)tmp_rot[1];
     m1.m02 = (float)tmp_rot[2];
     m1.m10 = (float)tmp_rot[3];
     m1.m11 = (float)tmp_rot[4];
     m1.m12 = (float)tmp_rot[5];
     m1.m20 = (float)tmp_rot[6];
     m1.m21 = (float)tmp_rot[7];
     m1.m22 = (float)tmp_rot[8];
 }
Ejemplo n.º 17
0
 /// <summary>
 /// Sets the value of this matrix to the matrix difference
 /// of matrices m1 and m2.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the matrix difference
 /// of matrices m1 and m2.
 /// </remarks>
 /// <param name="m1">the first matrix</param>
 /// <param name="m2">the second matrix</param>
 public void Sub(Matrix3f m1, Matrix3f m2)
 {
     this.m00 = m1.m00 - m2.m00;
     this.m01 = m1.m01 - m2.m01;
     this.m02 = m1.m02 - m2.m02;
     this.m10 = m1.m10 - m2.m10;
     this.m11 = m1.m11 - m2.m11;
     this.m12 = m1.m12 - m2.m12;
     this.m20 = m1.m20 - m2.m20;
     this.m21 = m1.m21 - m2.m21;
     this.m22 = m1.m22 - m2.m22;
 }
Ejemplo n.º 18
0
 /// <summary>
 /// Performs an SVD normalization of this matrix to calculate
 /// the rotation as a 3x3 matrix, the translation, and the scale.
 /// </summary>
 /// <remarks>
 /// Performs an SVD normalization of this matrix to calculate
 /// the rotation as a 3x3 matrix, the translation, and the scale.
 /// None of the matrix values are modified.
 /// </remarks>
 /// <param name="m1">the normalized matrix representing the rotation</param>
 /// <param name="t1">the translation component</param>
 /// <returns>the scale component of this transform</returns>
 public float Get(Matrix3f m1, Vector3f t1)
 {
     double[] tmp_rot = new double[9];
     // scratch matrix
     double[] tmp_scale = new double[3];
     // scratch matrix
     GetScaleRotate(tmp_scale, tmp_rot);
     m1.m00 = (float)tmp_rot[0];
     m1.m01 = (float)tmp_rot[1];
     m1.m02 = (float)tmp_rot[2];
     m1.m10 = (float)tmp_rot[3];
     m1.m11 = (float)tmp_rot[4];
     m1.m12 = (float)tmp_rot[5];
     m1.m20 = (float)tmp_rot[6];
     m1.m21 = (float)tmp_rot[7];
     m1.m22 = (float)tmp_rot[8];
     t1.x = m03;
     t1.y = m13;
     t1.z = m23;
     return ((float)Matrix3d.Max3(tmp_scale));
 }
Ejemplo n.º 19
0
 /// <summary>Sets the value of this matrix to the matrix sum of matrices m1 and m2.</summary>
 /// <remarks>Sets the value of this matrix to the matrix sum of matrices m1 and m2.</remarks>
 /// <param name="m1">the first matrix</param>
 /// <param name="m2">the second matrix</param>
 public void Add(Matrix3f m1, Matrix3f m2)
 {
     this.m00 = m1.m00 + m2.m00;
     this.m01 = m1.m01 + m2.m01;
     this.m02 = m1.m02 + m2.m02;
     this.m10 = m1.m10 + m2.m10;
     this.m11 = m1.m11 + m2.m11;
     this.m12 = m1.m12 + m2.m12;
     this.m20 = m1.m20 + m2.m20;
     this.m21 = m1.m21 + m2.m21;
     this.m22 = m1.m22 + m2.m22;
 }
Ejemplo n.º 20
0
 /// <summary>
 /// Sets the value of this quaternion to the rotational component of
 /// the passed matrix.
 /// </summary>
 /// <remarks>
 /// Sets the value of this quaternion to the rotational component of
 /// the passed matrix.
 /// </remarks>
 /// <param name="m1">the matrix3f</param>
 public void Set(Matrix3f m1)
 {
     double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0);
     if (ww >= 0)
     {
         if (ww >= Eps2)
         {
             this.w = Math.Sqrt(ww);
             ww = 0.25 / this.w;
             this.x = ((m1.m21 - m1.m12) * ww);
             this.y = ((m1.m02 - m1.m20) * ww);
             this.z = ((m1.m10 - m1.m01) * ww);
             return;
         }
     }
     else
     {
         this.w = 0;
         this.x = 0;
         this.y = 0;
         this.z = 1;
         return;
     }
     this.w = 0;
     ww = -0.5 * (m1.m11 + m1.m22);
     if (ww >= 0)
     {
         if (ww >= Eps2)
         {
             this.x = Math.Sqrt(ww);
             ww = 0.5 / this.x;
             this.y = (m1.m10 * ww);
             this.z = (m1.m20 * ww);
             return;
         }
     }
     else
     {
         this.x = 0;
         this.y = 0;
         this.z = 1;
         return;
     }
     this.x = 0;
     ww = 0.5 * (1.0 - m1.m22);
     if (ww >= Eps2)
     {
         this.y = Math.Sqrt(ww);
         this.z = (m1.m21 / (2.0 * this.y));
     }
     this.y = 0;
     this.z = 1;
 }
Ejemplo n.º 21
0
 /// <summary>
 /// Sets the value of this matrix to the matrix sum of itself and
 /// matrix m1.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the matrix sum of itself and
 /// matrix m1.
 /// </remarks>
 /// <param name="m1">the other matrix</param>
 public void Add(Matrix3f m1)
 {
     this.m00 += m1.m00;
     this.m01 += m1.m01;
     this.m02 += m1.m02;
     this.m10 += m1.m10;
     this.m11 += m1.m11;
     this.m12 += m1.m12;
     this.m20 += m1.m20;
     this.m21 += m1.m21;
     this.m22 += m1.m22;
 }
Ejemplo n.º 22
0
 /// <summary>
 /// Multiplies matrix m1 times the transpose of matrix m2, and
 /// places the result into this.
 /// </summary>
 /// <remarks>
 /// Multiplies matrix m1 times the transpose of matrix m2, and
 /// places the result into this.
 /// </remarks>
 /// <param name="m1">the matrix on the left hand side of the multiplication</param>
 /// <param name="m2">the matrix on the right hand side of the multiplication</param>
 public void MulTransposeRight(Matrix3f m1, Matrix3f m2)
 {
     if (this != m1 && this != m2)
     {
         this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
         this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
         this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
         this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
         this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
         this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
         this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
         this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
         this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
     }
     else
     {
         float m00;
         float m01;
         float m02;
         float m10;
         float m11;
         float m12;
         float m20;
         float m21;
         float m22;
         // vars for temp result matrix
         m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
         m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
         m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
         m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
         m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
         m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
         m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
         m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
         m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
         this.m00 = m00;
         this.m01 = m01;
         this.m02 = m02;
         this.m10 = m10;
         this.m11 = m11;
         this.m12 = m12;
         this.m20 = m20;
         this.m21 = m21;
         this.m22 = m22;
     }
 }
Ejemplo n.º 23
0
 /// <summary>
 /// Returns true if the L-infinite distance between this matrix
 /// and matrix m1 is less than or equal to the epsilon parameter,
 /// otherwise returns false.
 /// </summary>
 /// <remarks>
 /// Returns true if the L-infinite distance between this matrix
 /// and matrix m1 is less than or equal to the epsilon parameter,
 /// otherwise returns false.  The L-infinite
 /// distance is equal to
 /// MAX[i=0,1,2 ; j=0,1,2 ; abs(this.m(i,j) - m1.m(i,j)]
 /// </remarks>
 /// <param name="m1">the matrix to be compared to this matrix</param>
 /// <param name="epsilon">the threshold value</param>
 public virtual bool EpsilonEquals(Matrix3f m1, float epsilon)
 {
     bool status = true;
     if (Math.Abs(this.m00 - m1.m00) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m01 - m1.m01) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m02 - m1.m02) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m10 - m1.m10) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m11 - m1.m11) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m12 - m1.m12) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m20 - m1.m20) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m21 - m1.m21) > epsilon)
     {
         status = false;
     }
     if (Math.Abs(this.m22 - m1.m22) > epsilon)
     {
         status = false;
     }
     return (status);
 }
Ejemplo n.º 24
0
 /// <summary>
 /// Sets the value of this matrix equal to the negation of
 /// of the Matrix3f parameter.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix equal to the negation of
 /// of the Matrix3f parameter.
 /// </remarks>
 /// <param name="m1">the source matrix</param>
 public void Negate(Matrix3f m1)
 {
     this.m00 = -m1.m00;
     this.m01 = -m1.m01;
     this.m02 = -m1.m02;
     this.m10 = -m1.m10;
     this.m11 = -m1.m11;
     this.m12 = -m1.m12;
     this.m20 = -m1.m20;
     this.m21 = -m1.m21;
     this.m22 = -m1.m22;
 }
Ejemplo n.º 25
0
 /// <summary>
 /// Sets the value of this matrix to the matrix inverse
 /// of the passed matrix m1.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the matrix inverse
 /// of the passed matrix m1.
 /// </remarks>
 /// <param name="m1">the matrix to be inverted</param>
 public void Invert(Matrix3f m1)
 {
     InvertGeneral(m1);
 }
Ejemplo n.º 26
0
 /// <summary>
 /// Perform cross product normalization of matrix m1 and place the
 /// normalized values into this.
 /// </summary>
 /// <remarks>
 /// Perform cross product normalization of matrix m1 and place the
 /// normalized values into this.
 /// </remarks>
 /// <param name="m1">Provides the matrix values to be normalized</param>
 public void NormalizeCP(Matrix3f m1)
 {
     float mag = 1.0f / (float)Math.Sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10 + m1.m20 *
         m1.m20);
     m00 = m1.m00 * mag;
     m10 = m1.m10 * mag;
     m20 = m1.m20 * mag;
     mag = 1.0f / (float)Math.Sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11 + m1.m21 * m1.m21
         );
     m01 = m1.m01 * mag;
     m11 = m1.m11 * mag;
     m21 = m1.m21 * mag;
     m02 = m10 * m21 - m11 * m20;
     m12 = m01 * m20 - m00 * m21;
     m22 = m00 * m11 - m01 * m10;
 }
Ejemplo n.º 27
0
 /// <summary>
 /// Sets the value of this matrix to the result of multiplying itself
 /// with matrix m1.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the result of multiplying itself
 /// with matrix m1.
 /// </remarks>
 /// <param name="m1">the other matrix</param>
 public void Mul(Matrix3f m1)
 {
     float m00;
     float m01;
     float m02;
     float m10;
     float m11;
     float m12;
     float m20;
     float m21;
     float m22;
     m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
     m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
     m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
     m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
     m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
     m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
     m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
     m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
     m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
     this.m00 = m00;
     this.m01 = m01;
     this.m02 = m02;
     this.m10 = m10;
     this.m11 = m11;
     this.m12 = m12;
     this.m20 = m20;
     this.m21 = m21;
     this.m22 = m22;
 }
Ejemplo n.º 28
0
 /// <summary>
 /// Sets the value of this matrix to the value of the Matrix3f
 /// argument.
 /// </summary>
 /// <remarks>
 /// Sets the value of this matrix to the value of the Matrix3f
 /// argument.
 /// </remarks>
 /// <param name="m1">the source matrix3f</param>
 public void Set(Matrix3f m1)
 {
     this.m00 = m1.m00;
     this.m01 = m1.m01;
     this.m02 = m1.m02;
     this.m10 = m1.m10;
     this.m11 = m1.m11;
     this.m12 = m1.m12;
     this.m20 = m1.m20;
     this.m21 = m1.m21;
     this.m22 = m1.m22;
 }
Ejemplo n.º 29
0
 /// <summary>
 /// Multiplies matrix m1 by matrix m2, does an SVD normalization
 /// of the result, and places the result into this matrix.
 /// </summary>
 /// <remarks>
 /// Multiplies matrix m1 by matrix m2, does an SVD normalization
 /// of the result, and places the result into this matrix.
 /// this = SVDnorm(m1*m2).
 /// </remarks>
 /// <param name="m1">the matrix on the left hand side of the multiplication</param>
 /// <param name="m2">the matrix on the right hand side of the multiplication</param>
 public void MulNormalize(Matrix3f m1, Matrix3f m2)
 {
     double[] tmp = new double[9];
     // scratch matrix
     double[] tmp_rot = new double[9];
     // scratch matrix
     double[] tmp_scale = new double[3];
     // scratch matrix
     tmp[0] = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
     tmp[1] = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
     tmp[2] = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
     tmp[3] = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
     tmp[4] = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
     tmp[5] = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
     tmp[6] = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
     tmp[7] = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
     tmp[8] = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
     Matrix3d.Compute_svd(tmp, tmp_scale, tmp_rot);
     this.m00 = (float)(tmp_rot[0]);
     this.m01 = (float)(tmp_rot[1]);
     this.m02 = (float)(tmp_rot[2]);
     this.m10 = (float)(tmp_rot[3]);
     this.m11 = (float)(tmp_rot[4]);
     this.m12 = (float)(tmp_rot[5]);
     this.m20 = (float)(tmp_rot[6]);
     this.m21 = (float)(tmp_rot[7]);
     this.m22 = (float)(tmp_rot[8]);
 }
Ejemplo n.º 30
0
 /// <summary>
 /// Constructs and initializes a Matrix4d from the rotation matrix,
 /// translation, and scale values; the scale is applied only to the
 /// rotational components of the matrix (upper 3x3) and not to the
 /// translational components of the matrix.
 /// </summary>
 /// <remarks>
 /// Constructs and initializes a Matrix4d from the rotation matrix,
 /// translation, and scale values; the scale is applied only to the
 /// rotational components of the matrix (upper 3x3) and not to the
 /// translational components of the matrix.
 /// </remarks>
 /// <param name="m1">the rotation matrix representing the rotational components</param>
 /// <param name="t1">the translational components of the matrix</param>
 /// <param name="s">the scale value applied to the rotational components</param>
 public Matrix4d(Matrix3f m1, Vector3d t1, double s)
 {
     this.m00 = m1.m00 * s;
     this.m01 = m1.m01 * s;
     this.m02 = m1.m02 * s;
     this.m03 = t1.x;
     this.m10 = m1.m10 * s;
     this.m11 = m1.m11 * s;
     this.m12 = m1.m12 * s;
     this.m13 = t1.y;
     this.m20 = m1.m20 * s;
     this.m21 = m1.m21 * s;
     this.m22 = m1.m22 * s;
     this.m23 = t1.z;
     this.m30 = 0.0;
     this.m31 = 0.0;
     this.m32 = 0.0;
     this.m33 = 1.0;
 }