C++ SquareMatrix类代码示例

void solveCircleSummation() {
	int nTests;
	scanf(" %d", &nTests);
	for(int test = 0; test < nTests; ++test) {
		int N;
		int mSize;
		scanf(" %d %d", &N, &mSize);

		vector<ullong> vec;
		for(int entry = 0; entry < N; ++entry) {
			int inp;
			scanf(" %d", &inp);
			vec.push_back(inp);
		}
		//printf("%d %d\n",N, mSize);
		SquareMatrix sqMat = SquareMatrix(N, mSize);
		int N_1 = mSize%N;
		vector<ullong> finalOut(N, 0);
		for(int entry = 0; entry < N; ++entry) {
			sqMat.multiply(vec, finalOut);
			print(finalOut, N_1);
			printf("\n");
			N_1 = (N_1 + 1) % N;
			rotateBy1(vec);
		}
		if(test < nTests -1)
			printf("\n");
	}
}

TEST_F(TestInterReflectanceBSDF, TestBSDFInterreflectance)
{
    SCOPED_TRACE("Begin Test: Simple BSDF interreflectance.");

    CInterReflectance interRefl = *getInterReflectance();

    SquareMatrix results = interRefl.value();

    const size_t matrixSize = results.size();

    // Test matrix
    size_t size = 7;

    EXPECT_EQ(size, matrixSize);

    SquareMatrix correctResults{{1.005964363, 0, 0, 0, 0, 0, 0},
                                {0, 1.005964363, 0, 0, 0, 0, 0},
                                {0, 0, 1.006280195, 0, 0, 0, 0},
                                {0, 0, 0, 1.008724458, 0, 0, 0},
                                {0, 0, 0, 0, 1.021780268, 0, 0},
                                {0, 0, 0, 0, 0, 1.176150952, 0},
                                {0, 0, 0, 0, 0, 0, 3.022280250}};

    for(size_t i = 0; i < size; ++i)
    {
        for(size_t j = 0; j < size; ++j)
        {
            EXPECT_NEAR(correctResults(i, j), results(i, j), 1e-6);
        }
    }
}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates and returns a matrix that represents the inverse of this SquareMatrix.
*/
SquareMatrix * SquareMatrix::Inverse() const
    {
    double * col = new double[dim];
    int * permutation = new int[dim];
    SquareMatrix * tmp = new SquareMatrix(*this);
    double ** a = tmp->GetMatrixAsRawPointer();
    SquareMatrix * inv = new SquareMatrix(*this);
    double ** a_inv = inv->GetMatrixAsRawPointer();

    // double **  a           matrix represented as vector of row pointers
    // int        n           order of matrix
    // double *   col         work vector of size n
    // int *      permutation work vector of size n
    // double **  a_inv       inverse of input matrix a (matrix a is destroyed)
    int result = InvertMatrix(a, dim, col, permutation, a_inv);
    delete tmp;
    delete [] col;
    delete [] permutation;
    if (result != 0)
        {
        delete inv;
        return 0;
        }
    return inv;
    }

void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{
    int* rowStarts = (int*)mxGetData(prhs[0]);
    int* colIdx = (int*)mxGetData(prhs[1]);
    double* values = (double*)mxGetData(prhs[2]);
    int n = (int)*mxGetPr(prhs[3]);
    int nnz = (int)*mxGetPr(prhs[4]);
    double *x = (double*)mxGetData(prhs[5]);
    int *startsOut, *colsOut;
    double *valOut;
    double thetta  = (double)*mxGetPr(prhs[6]);
    SquareMatrix* S = new SquareMatrix(n,nnz, rowStarts, colIdx, values);
    multiplyDiagonalFromLeft(S,x);
    double* mins = new double[n];
    getMinimumsByRow(S, mins);
    zeroLowerThanThetaMaxInRow(S, mins, thetta);
    SquareMatrix* St = trimAndTranspose(S);
    int symNNZ = nnzInSymmetrization(S, St);
    delete mins;
    const int dims[] = {1,n};
    const int dimsNNZ[] = {1,symNNZ};
    plhs[0] = mxCreateNumericArray(2,dims,mxINT32_CLASS,mxREAL);
    plhs[1] = mxCreateNumericArray(2,dimsNNZ,mxINT32_CLASS,mxREAL);
    plhs[2] = mxCreateDoubleMatrix(1, symNNZ, mxREAL);
    startsOut = (int*)mxGetData(plhs[0]);
    colsOut = (int*)mxGetData(plhs[1]);
    valOut = mxGetPr(plhs[2]);
    symmetrisizeAndTrim(S, St, colsOut, valOut, startsOut, n, symNNZ);
    delete S;
    St->freeArrays();
    delete St;
}

Scalar IsotropicLinearElasticity<Scalar,Dim>::energy(const SquareMatrix<Scalar,Dim> &F) const
{
    SquareMatrix<Scalar,Dim> e = 0.5*(F.transpose()+F)-SquareMatrix<Scalar,Dim>::identityMatrix();
    Scalar trace_e = e.trace();
    Scalar lambda = this->lambda_;
    Scalar mu = this->mu_;
    Scalar energy = 0.5*lambda*trace_e*trace_e+mu*e.doubleContraction(e);
    return energy;
}

SquareMatrix<Scalar,Dim> NeoHookean<Scalar,Dim>::secondPiolaKirchhoffStress(const SquareMatrix<Scalar,Dim> &F) const
{
    SquareMatrix<Scalar,Dim> identity = SquareMatrix<Scalar,Dim>::identityMatrix();
    SquareMatrix<Scalar,Dim> inverse_c = (F.transpose()*F).inverse();
    Scalar lnJ = log(F.determinant());
    Scalar mu = this->mu_;
    Scalar lambda = this->lambda_;
    SquareMatrix<Scalar,Dim> S = mu*(identity-inverse_c)+lambda*lnJ*inverse_c;
    return S;
}

void BlockLocalPositionEstimator::landCorrect()
{
	// measure land
	Vector<float, n_y_land> y;

	if (landMeasure(y) != OK) { return; }

	// measurement matrix
	Matrix<float, n_y_land, n_x> C;
	C.setZero();
	// y = -(z - tz)
	C(Y_land_vx, X_vx) = 1;
	C(Y_land_vy, X_vy) = 1;
	C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir.
	C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir.

	// use parameter covariance
	SquareMatrix<float, n_y_land> R;
	R.setZero();
	R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
	R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
	R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();

	// residual
	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
	Vector<float, n_y_land> r = y - C * _x;
	_pub_innov.get().hagl_innov = r(Y_land_agl);
	_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);

	// fault detection
	float beta = (r.transpose() * (S_I * r))(0, 0);

	// artifically increase beta threshhold to prevent fault during landing
	float beta_thresh = 1e2f;

	if (beta / BETA_TABLE[n_y_land] > beta_thresh) {
		if (!(_sensorFault & SENSOR_LAND)) {
			_sensorFault |= SENSOR_LAND;
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault,  beta %5.2f", double(beta));
		}

		// abort correction
		return;

	} else if (_sensorFault & SENSOR_LAND) {
		_sensorFault &= ~SENSOR_LAND;
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
	}

	// kalman filter correction always for land detector
	Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
	Vector<float, n_x> dx = K * r;
	_x += dx;
	_P -= K * C * _P;
}

SquareMatrix<Scalar,Dim> IsotropicLinearElasticity<Scalar,Dim>::firstPiolaKirchhoffStressDifferential(
                                                                const SquareMatrix<Scalar,Dim> &F,
                                                                const SquareMatrix<Scalar,Dim> &F_differential) const
{
    Scalar mu = this->mu_;
    Scalar lambda = this->lambda_;
    SquareMatrix<Scalar,Dim> identity = SquareMatrix<Scalar,Dim>::identityMatrix();
    SquareMatrix<Scalar,Dim> e = 0.5*(F.transpose()+F)-identity;
    SquareMatrix<Scalar,Dim> e_differential = 0.5*(F_differential.transpose()+F_differential);
    return 2.0*mu*e_differential+lambda*e_differential.trace()*identity;
}

SquareMatrix SquareMatrix::operator+(const SquareMatrix & rhs)const
{
    if (getSize() != rhs.getSize())
        throw "Matrix must be equal size!!!";

    SquareMatrix result(*this);
    for (unsigned int i = 0; i < result.getSize(); i++)
        for (unsigned int j = 0; j < result.getSize(); j++)
            result[i][j] += rhs.get(i, j);
    return result;
}

void ValidateIdentity(const AffineTransform & t) {
  SquareMatrix m = t.GetMatrix();
  for (unsigned int row = 0; row < m.getRows(); row++) {
    for (unsigned int col = 0; col < m.getCols(); col++) {
      if (row == col) {
        ASSERT_NEAR(1, m[row][col], ABS_ERR);
      } else {
        ASSERT_NEAR(0, m[row][col], ABS_ERR);
      }
    }
  }
}

SquareMatrix pow(SquareMatrix m, int n)
{
	SquareMatrix res;

	res.init();
	for (; n > 0; n = n >> 1) {
		if (n & 1) {
			res = res * m;
		}
		m = m * m;
	}
	return res;
}

double det(const SquareMatrix &a, double EPS = 1e-10) {
  SquareMatrix b(a);
  int n = a.size();
  double res = 1.0;
  std::vector<bool> used(n, false);
  for (int i = 0; i < n; i++) {
    int p;
    for (p = 0; p < n; p++) {
      if (!used[p] && fabs(b[p][i]) > EPS) {
        break;
      }
    }
    if (p >= n) {
      return 0;
    }
    res *= b[p][i];
    used[p] = true;
    double z = 1.0/b[p][i];
    for (int j = 0; j < n; j++) {
      b[p][j] *= z;
    }
    for (int j = 0; j < n; j++) {
      if (j != p) {
        z = b[j][i];
        for (int k = 0; k < n; k++) {
          b[j][k] -= z*b[p][k];
        }
      }
    }
  }
  return res;
}

double det_naive(const SquareMatrix &a) {
  int n = a.size();
  if (n == 1) {
    return a[0][0];
  }
  if (n == 2) {
    return a[0][0]*a[1][1] - a[0][1]*a[1][0];
  }
  double res = 0;
  SquareMatrix temp(n - 1, typename SquareMatrix::value_type(n - 1));
  for (int p = 0; p < n; p++) {
    int h = 0, k = 0;
    for (int i = 1; i < n; i++) {
      for (int j = 0; j < n; j++) {
        if (j == p) {
          continue;
        }
        temp[h][k++] = a[i][j];
        if (k == n - 1) {
          h++;
          k = 0;
        }
      }
    }
    res += (p % 2 == 0 ? 1 : -1)*a[0][p]*det_naive(temp);
  }
  return res;
}

bool operator==(const SquareMatrix &a, const SquareMatrix &b)
{
    if (a.dimensions() != b.dimensions())
    {
        return false;
    }

    size_t elementCount = a.elementCount();
    for (size_t i = 0; i < elementCount; ++i)
    {
        if (a.at(i) != b.at(i))
        {
            return false;
        }
    }
    return true;
}

Foam::scalar Foam::det(SquareMatrix<Type>& matrix)
{
    labelList pivotIndices(matrix.n());
    label sign;
    LUDecompose(matrix, pivotIndices, sign);

    return detDecomposed(matrix, sign);
}