C++ Matrix3x3类代码示例

Matrix3x3 CPhysicsActor::GetGlobalOrientation()
{
	Matrix3x3 orientation;
	NxMat33 tmp = m_Actor->getGlobalOrientation();
	tmp.getColumnMajor( orientation.GetMatrix() );
	return orientation;
}

//-----------------------------------------------------------------------
void Matrix3x3::EigenSolveSymmetric (double afEigenvalue[3],
    Vector3 akEigenvector[3]) const
{
    Matrix3x3 kMatrix = *this;
    double afSubDiag[3];
    kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
    kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);

    for (size_t i = 0; i < 3; i++)
    {
        akEigenvector[i][0] = kMatrix[0][i];
        akEigenvector[i][1] = kMatrix[1][i];
        akEigenvector[i][2] = kMatrix[2][i];
    }

    // make eigenvectors form a right--handed system
    Vector3 kCross = akEigenvector[1].CrossProduct(akEigenvector[2]);
    double fDet = akEigenvector[0].DotProduct(kCross);
    if ( fDet < 0.0 )
    {
        akEigenvector[2][0] = - akEigenvector[2][0];
        akEigenvector[2][1] = - akEigenvector[2][1];
        akEigenvector[2][2] = - akEigenvector[2][2];
    }
}

            void Quaternion::transformToMatrix4x4(double *matrix4x4) const {
                if (matrix4x4 != NULL) {
                    Matrix3x3 result = transformToMatrix3x3();

                    matrix4x4[0] = result.getXX();
                    matrix4x4[1] = result.getXY();
                    matrix4x4[2] = result.getXZ();
                    matrix4x4[3] = 0;

                    // Second row
                    matrix4x4[4] = result.getYX();
                    matrix4x4[5] = result.getYY();
                    matrix4x4[6] = result.getYZ();
                    matrix4x4[7] = 0;

                    // Third row
                    matrix4x4[8] = result.getZX();
                    matrix4x4[9] = result.getZY();
                    matrix4x4[10] = result.getZZ();
                    matrix4x4[11] = 0.;

                    // Fourth row
                    matrix4x4[12] = 0;
                    matrix4x4[13] = 0;
                    matrix4x4[14] = 0;
                    matrix4x4[15] = 1;
                }
            }

bool CMatrix4x4<T>::GetInverseMatrix(CMatrix4x4 &matInverse) const
{

	T nDeterminate = Determinate();

	int i, j, sign;

	if(fabs(nDeterminate) < 0.0005)
	{
		return false;
	}

	for(i = 0; i < 4; i++)
	{
		for(j = 0; j < 4; j++)
		{

			sign = 1 - ((i+j)%2) * 2;

			Matrix3x3<T> matSub = GetSubMatrix3x3(i,j);

			T det = matSub.Determinate();

			matInverse.m_Entries[i+j*4] = (det * sign)/nDeterminate;

		}//end for
	}//end for

	return true;
}//end GetInverseMatrix

   // Given an edge, the constructor for EdgeRecord finds the
   // optimal point associated with the edge's current quadric,
   // and assigns this edge a cost based on how much quadric
   // error is observed at this optimal point.
   EdgeRecord::EdgeRecord( EdgeIter& _edge )
   : edge( _edge )
   {
      // TODO Compute the combined quadric from the edge endpoints.
			Matrix4x4 q = _edge->halfedge()->vertex()->quadric +
										_edge->halfedge()->twin()->vertex()->quadric;
		 
      // TODO Build the 3x3 linear system whose solution minimizes
      // the quadric error associated with these two endpoints.
			Matrix3x3 quadratic;
			quadratic(0,0) =  q(0,0); quadratic(0,1) =  q(0,1); quadratic(0,2) =  q(0,2);
			quadratic(1,0) =  q(1,0); quadratic(1,1) =  q(1,1); quadratic(1,2) =  q(1,2);
			quadratic(2,0) =  q(2,0); quadratic(2,1) =  q(2,1); quadratic(2,2) =  q(2,2);
			Vector3D linear(q(3,0), q(3,1), q(3,2));
		 
      // TODO Use this system to solve for the optimal position, and
      // TODO store it in EdgeRecord::optimalPoint.
			optimalPoint = - quadratic.inv() * linear;
		 
      // TODO Also store the cost associated with collapsing this edge
      // TODO in EdgeRecord::Cost.
			Vector4D optH(optimalPoint);
			optH.w = 1.0;
			score = dot(optH, q * optH);
   }

void PPC::SetPMat(){
	Matrix3x3 cam;
	cam.setColumn(0, a);
	cam.setColumn(1, b);
	cam.setColumn(2, c);
	pMat = cam.inverse();
}

		Matrix3x3 &Matrix3x3::Clone( ) const
		{
			Matrix3x3 *pClone = new Matrix3x3( );

			pClone->Copy( *this );

			return *pClone;
		}

TEST(Matrix3x3, Sum)
{
    const Matrix3x3<int> matrix1(1, 2, 3, 4, 5, 6, 7, 8, 9);
    const Matrix3x3<double> matrix2(10.08, 11.10, 24.5, 24.0, 30.70, 50.50, 70.90, 80.05, 100.70);

    EXPECT_EQ(matrix1.sum(), 45);
    EXPECT_EQ(matrix2.sum(), 402.53);
}

void Node::setRotation( const Matrix3x3& rotation )
{
	assert( rotation.getColumn(0).finite() );
	assert( rotation.getColumn(1).finite() );
	assert( rotation.getColumn(2).finite() );
	assert( ( rotation.getColumn(0).cross(rotation.getColumn(1)) ).dot( rotation.getColumn(2) ) > 0.f ); // Left handed coordinate system?

	m_localTransform.setRotation( rotation );
	m_flags |= NODE_WORLDTMDIRTY;
}

TEST(M3x3, opMulti)
{
	Matrix3x3 testMatrix0;
	testMatrix0.FillMatrix(1,2,2,3,4,5,6,7,9);
	Matrix3x3 testMatrix1;
	testMatrix1.FillMatrix(4, 1, 2, 3, 6, 3, 2, 1, 7);
	Matrix3x3 result;
	result.FillMatrix(14, 15, 22, 34, 32, 53, 63, 57, 96);

	EXPECT_EQ(result, (testMatrix0*testMatrix1));
}

            Point3 Point3::operator*(const Matrix3x3 &m) const {
                const double x = getX() * m.getXX() + getY() * m.getXY() + getZ() * m.getXZ();
                const double y = getX() * m.getYX() + getY() * m.getYY() + getZ() * m.getYZ();
                const double z = getX() * m.getZX() + getY() * m.getZY() + getZ() * m.getZZ();

                Point3 cc(x, y, z);
                return cc;
            }

Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &right) const
{
	Matrix3x3 ret;
	for (unsigned int iRow = 0; iRow < 3; iRow++)
	{
		for (unsigned int iCol = 0; iCol < 3; iCol++)
		{
			ret.SetCell(iRow, iCol, this->GetRow(iRow) * right.GetColumn(iCol));
		}
	}
	return ret;
}

//check if points of the triangle is counterclockwise
bool cclock( float x0, float y0,
             float x1, float y1,
             float x2, float y2 ) {
  Matrix3x3 m;
  m(0,0) = x0; m(0,1) = x1; m(0,2) = x2;
  m(1,0) = y0; m(1,1) = y1; m(1,2) = y2;
  m(2,0) = 1 ; m(2,1) = 1 ; m(2,2) = 1 ;
  return m.det() >= 0.0;
  /*
  return lineSide(x2 - x0, y2 - y0, x0, y0, x1, y1);
  */
}

// this should be moved to the math classes
// NEED NEW VEC CLASS
Vec3 rotateVectorAxisAngle( const Vec3 &vec, const Vec3 &axis, const float angle )
{
    Matrix3x3 mat;
    mat.SetIdentity();
    mat.SetFromAxisAngle( axis, angle );
    Vec3 transformed;
    //need to check the matrix type here carefully
    transformed.x = mat.m[0]*vec.x + mat.m[1]*vec.y + mat.m[2]*vec.z;
    transformed.y = mat.m[3]*vec.x + mat.m[4]*vec.y + mat.m[5]*vec.z;
    transformed.z = mat.m[6]*vec.x + mat.m[7]*vec.y + mat.m[8]*vec.z;
    return transformed;
}

Matrix3x3 Matrix3x3::MulMatrix(Matrix3x3 matrix)
{
	Matrix3x3 ret;
	Matrix3x1 col;

	for(int i = 0; i < 3; i++)
	{
		col = this->MulMatrix3X1(matrix.GetColumn(i));
		ret.SetColumn(col, i);
	}

	return ret;
}