C++ Matrix::GetRow方法代码示例

void OpenGL::DrawLine(const Matrix& p, const Matrix& q, double width)
{
	if ( (p.GetRow() != 3) || (p.GetCol() != 1) || (q.GetRow() != 3) || (q.GetCol() != 1) )
	{
		fprintf(stderr, "OpenGL::DrawLine Invalid Matrix");
		return;
	}

	DrawLine( p(1), p(2), p(3), q(1), q(2), q(3), width );

	return;
}

void OpenGL::DrawFrame(const Matrix& p, const Matrix& q)
{
	if ( (p.GetRow() != 3) || (p.GetCol() != 1) || (q.GetRow() != 3) || (q.GetCol() != 1) )
	{
		fprintf(stderr, "OpenGL::DrawFrame Invalid Matrix");
		return;
	}

	DrawFrame( p(1), p(2), p(3), q(1), q(2), q(3) );

	return;

}

void OpenGL::DrawPolygonSurface(const Matrix& p, const Matrix& q, const Matrix& r)
{
	if ( (p.GetRow() != 3) || (p.GetCol() != 1) ||
		 (q.GetRow() != 3) || (q.GetCol() != 1) ||
		 (r.GetRow() != 3) || (r.GetCol() != 1) )
	{
		fprintf(stderr, "OpenGL::DrawPolygonSurface Invalid Matrix\n\n");
		return;
	}

	DrawPolygonSurface( p(1), p(2), p(3), q(1), q(2), q(3), r(1), r(2), r(3) );

	return;
}

    void itr_algorithm::GaussianNaiveBayes::TrainPos(const Matrix &input)
    {
        int length = input.GetRow();
        F32 *data = new F32[length];
        F32 mu, sigma;

        if (!initpos)
        {
            for (int i = 0; i < featureNum; i++)
            {
                input.CopyColTo(i, data);
                itr_math::StatisticsObj->Mean(data, length, muPos[i]);
                itr_math::StatisticsObj->Variance(data, length, sigmaPos[i]);
            }
            initpos = true;
        }
        else
        {
            for (int i = 0; i < featureNum; i++)
            {
                input.CopyColTo(i, data);
                itr_math::StatisticsObj->Mean(data, length, mu);
                itr_math::StatisticsObj->Variance(data, length, sigma);

                itr_math::NumericalObj->Sqrt(
                    LearnRate * sigmaPos[i] * sigmaPos[i] + (1 - LearnRate) * sigma * sigma
                    + LearnRate * (1 - LearnRate) * (mu - muPos[i]) * (mu - muPos[i]),
                    sigmaPos[i]);
                muPos[i] = LearnRate * muPos[i] + (1 - LearnRate) * mu;
            }

        }
    }

int Encoder(int res[], Matrix key, int result[], int length) {
	int cal_length = key.GetRow();
	if (length % cal_length != 0) {
		int padding = (length / cal_length + 1) * cal_length - length;
		int group_num = length / cal_length + 1;
		for (int i = 0; i < padding; i++) 
			res[length + i] = -1;
		for (int k = 0; k < group_num; k++) {
			for (int i = 0; i < cal_length; i++) {
				int temp_res[cal_length], temp_result[cal_length];
				for (int j = 0; j < cal_length; j++)
					temp_res[j] = res[j + cal_length * k];
				DetailEncoder(temp_res, key, temp_result, cal_length);
				for (int j = 0; j < cal_length; j++)
					result[cal_length * k + j] = temp_result[j];
			}
		}
	}
	else {
		int group_num = length / cal_length;
		for (int k = 0; k < group_num; k++) {
			for (int i = 0; i < cal_length; i++) {
				int temp_res[cal_length], temp_result[cal_length];
				for (int j = 0; j < cal_length; j++)
					temp_res[j] = res[j + cal_length * k];
				DetailEncoder(temp_res, key, temp_result, cal_length);
				for (int j = 0; j < cal_length; j++)
					result[cal_length * k + j] = temp_result[j];
			}
		}
	}
}

//Used for Cascaded Shadow Mapping to calculate shadow cascades.
//This function doesn't calculate frustum's planes
void Camera::SplitFrustum(float zn, float zf, Frustum* pFrustum)
{
    Matrix matrix = mOwner->mMatrix * mParentOffset;

    Vector xAxis, yAxis, zAxis;
    Vector pos = matrix.GetRow(3);

    if (projMode == ProjectionMode::Perspective)
    {
        float y = tanf(perspective.FoV / 2.0f);
        float x = perspective.aspectRatio * y;
        xAxis = x * matrix.GetRow(0);
        yAxis = y * matrix.GetRow(1);
        zAxis = matrix.GetRow(2);

        pFrustum->verticies[0] = pos + zn * (zAxis - xAxis - yAxis);
        pFrustum->verticies[1] = pos + zn * (zAxis + xAxis - yAxis);
        pFrustum->verticies[2] = pos + zn * (zAxis - xAxis + yAxis);
        pFrustum->verticies[3] = pos + zn * (zAxis + xAxis + yAxis);

        pFrustum->verticies[4] = pos + zf * (zAxis - xAxis - yAxis);
        pFrustum->verticies[5] = pos + zf * (zAxis + xAxis - yAxis);
        pFrustum->verticies[6] = pos + zf * (zAxis - xAxis + yAxis);
        pFrustum->verticies[7] = pos + zf * (zAxis + xAxis + yAxis);
    }
    else
    {
        xAxis = matrix.GetRow(0);
        yAxis = matrix.GetRow(1);
        zAxis = matrix.GetRow(2);

        pFrustum->verticies[0] = pos + zn * zAxis + ortho.left * xAxis  + ortho.bottom * yAxis;
        pFrustum->verticies[1] = pos + zn * zAxis + ortho.right * xAxis + ortho.bottom * yAxis;
        pFrustum->verticies[2] = pos + zn * zAxis + ortho.left * xAxis  + ortho.top * yAxis;
        pFrustum->verticies[3] = pos + zn * zAxis + ortho.right * xAxis + ortho.top * yAxis;

        pFrustum->verticies[4] = pos + zf * zAxis + ortho.left * xAxis  + ortho.bottom * yAxis;
        pFrustum->verticies[5] = pos + zf * zAxis + ortho.right * xAxis + ortho.bottom * yAxis;
        pFrustum->verticies[6] = pos + zf * zAxis + ortho.left * xAxis  + ortho.top * yAxis;
        pFrustum->verticies[7] = pos + zf * zAxis + ortho.right * xAxis + ortho.top * yAxis;
    }
}

void Draw::Correspond(const Matrix &Img1, const Matrix &Img2,
                      const vector<Point2D> &feature1, const vector<Point2D> &feature2,S32 FeatureNum,
                      Matrix &Result)
{
    Result.Init(Img1.GetRow() , Img1.GetCol()* 2 + 10);
    int offset = Img1.GetCol() + 10;
    for (int i = 0; i < Img1.GetCol(); i++)
        for (int j = 0; j < Img1.GetRow(); j++)
        {
            Result(j, i) = Img1(j, i);
            Result(j, i + offset) = Img2(j, i);
        }
    for (int i = 0; i < FeatureNum; i++)
    {
        Line(Result, feature1[i].X, feature1[i].Y, feature2[i].X + offset, feature2[i].Y,
             255);
        Circle(Result,feature1[i].X, feature1[i].Y,2,255);
        Circle(Result,feature2[i].X + offset, feature2[i].Y,2,255);
    }
}

static void printMatrix(Matrix a)
{
    for (int i = 0; i < a.GetRow(); ++i)
    {
        for (int j = 0; j < a.GetCol(); ++j)
        {
            printf("%f ", a(i, j));
        }
        printf("\n");
    }
    printf("\n");
}

// Encoder
int DetailEncoder(int res[], Matrix key, int result[], int length) {
	// Initialization
	for (int i = 0; i < length; i++)
		result[i] = 0;
	// Start encoding
	for (int i = 0; i < key.GetCol(); i++) {
		for (int j = 0; j < key.GetRow(); j++)
			result[i] = result[i] + res[j] * key.GetValue(j, i);
	}
	for (int i = 0; i < length; i++)
		result[i] = result[i] % 26;
}

void Matrix::operator =(Matrix& rhs) {
	for (int i = 1; i <= 4; i++)
		this->SetRow(rhs.GetRow(i), i);
}

void FubiGMR::calculateGMR(std::vector<Fubi::Vec3f>& means, std::vector<Fubi::Matrix3f>& inverseCovs)
{
	// Helping vars for extracting the right columns (t, x, y, z)
	int outDataSize = means.size();
	const int outDim = 3;
	Vector inComponents(1), outComponents(outDim);
	inComponents(0) = 0;
	for (unsigned int i = 0; i < 3; i++)
		outComponents(i) = (float)(i + 1);
	Matrix inData(outDataSize, 1);
	float* inArray = inData.Array();
	for (int i = 0; i < outDataSize; ++i)
	{
		inArray[i] = (float)i;
	}

	// Calculate probabilites for each data point
	Matrix Pxi(outDataSize, m_numStates);
	for (int i = 0; i < outDataSize; i++)
	{
		float norm_f = 0.0f;
		Vector row;
		for (int s = 0; s < m_numStates; s++)
		{
			float p_i = m_priors[s] * pdfState(inData.GetRow(i, row), inComponents, s);
			Pxi(i, s) = p_i;
			norm_f += p_i;
		}
		Pxi.SetRow(Pxi.GetRow(i) / norm_f, i);
	}

	// Calculate sigmas + variance for each state
	Matrix* subSigma = new Matrix[m_numStates];
	Matrix* subSigmaVar = new Matrix[m_numStates];
	for (int s = 0; s < m_numStates; s++)
	{
		Matrix isubSigmaIn;
		Matrix subSigmaOut;
		m_sigma[s].GetMatrixSpace(inComponents, inComponents, subSigmaOut);
		subSigmaOut.Inverse(isubSigmaIn);
		m_sigma[s].GetMatrixSpace(outComponents, inComponents, subSigmaOut);
		subSigma[s] = subSigmaOut*isubSigmaIn;
		m_sigma[s].GetMatrixSpace(outComponents, outComponents, subSigmaOut);
		m_sigma[s].GetMatrixSpace(inComponents, outComponents, isubSigmaIn);
		subSigmaVar[s] = subSigmaOut - subSigma[s] * isubSigmaIn;
	}

	// Now calculate the final mus and sigmas for each data point
	Matrix subMuIn;
	Matrix subMuOut;
	m_mu->GetColumnSpace(outComponents, subMuOut);
	m_mu->GetColumnSpace(inComponents, subMuIn);
	for (int i = 0; i < outDataSize; i++)
	{
		// Calculate mu and sigma for the current data point
		Matrix sigmaOut(outDim, outDim);
		Vector muOut(outDim, true);
		for (int s = 0; s < m_numStates; s++)
		{
			const float pIS = Pxi(i, s);
			muOut += (subMuOut.GetRow(s) + (subSigma[s] * (inData.GetRow(i) - subMuIn.GetRow(s)))) * pIS;
			sigmaOut += subSigmaVar[s] * (pIS*pIS);
		}

		// Copy them to the output vars
		Fubi::Matrix3f& inverseCov = inverseCovs[i];
		Fubi::Vec3f& mean = means[i];
		for (unsigned int j = 0; j < outDim; j++)
		{
			mean[j] = muOut[j];
			for (unsigned int k = 0; k < outDim; k++)
				inverseCov.c[j][k] = sigmaOut(j, k);
		}
		// Don't forget to invert the covariance matrix!
		inverseCov = inverseCov.inverted();
	}

	delete[] subSigma;
	delete[] subSigmaVar;
}

void Camera::OnUpdate(float dt)
{
    Matrix matrix = mParentOffset * mOwner->mMatrix;

    //calculate view matrix
    mViewMatrix = MatrixLookTo(matrix.r[3], matrix.r[2], matrix.r[1]);
    mViewMatrixInv = MatrixInverse(mViewMatrix);

    //calculat projection matrix
    if (projMode == ProjectionMode::Perspective)
        mProjMatrix = MatrixPerspective(perspective.aspectRatio, perspective.FoV,
                                        perspective.farDist, perspective.nearDist);
    else
        mProjMatrix = MatrixOrtho(ortho.left, ortho.right, ortho.bottom, ortho.top,
                                  ortho.nearDist, ortho.farDist);
    mProjMatrixInv = MatrixInverse(mProjMatrix);



    //calculate secondary view matrix for motion blur
    Matrix rotMatrix = MatrixRotationNormal(mOwner->mAngularVelocity, 0.01f);
    Matrix secondaryCameraMatrix;
    secondaryCameraMatrix.r[0] = VectorTransform3(matrix.r[0], rotMatrix);
    secondaryCameraMatrix.r[1] = VectorTransform3(matrix.r[1], rotMatrix);
    secondaryCameraMatrix.r[2] = VectorTransform3(matrix.r[2], rotMatrix);
    secondaryCameraMatrix.r[3] = (Vector)matrix.r[3] + mOwner->mVelocity * 0.01f;
    Matrix secondaryViewMatrix = MatrixLookTo(secondaryCameraMatrix.r[3], secondaryCameraMatrix.r[2],
                                 secondaryCameraMatrix.r[1]);
    mSecondaryProjViewMatrix = secondaryViewMatrix * mProjMatrix;

    /*
    //calculate secondary view matrix for motion blur
    Matrix prevViewMatrix = MatrixLookTo(mPrevMatrix.r[3], mPrevMatrix.r[2], mPrevMatrix.r[1]);
    mSecondaryProjViewMatrix = prevViewMatrix * mProjMatrix;
    */

    Vector xAxis, yAxis, zAxis;
    Vector pos = matrix.GetRow(3);

    if (projMode == ProjectionMode::Perspective)
    {
        float y = tanf(perspective.FoV / 2.0f);
        float x = perspective.aspectRatio * y;
        mScreenScale = Vector(x, y, 0, 0);

        xAxis = x * matrix.GetRow(0);
        yAxis = y * matrix.GetRow(1);
        zAxis = matrix.GetRow(2);

        mFrustum.verticies[0] = pos + perspective.nearDist * (zAxis - xAxis - yAxis);
        mFrustum.verticies[1] = pos + perspective.nearDist * (zAxis + xAxis - yAxis);
        mFrustum.verticies[2] = pos + perspective.nearDist * (zAxis - xAxis + yAxis);
        mFrustum.verticies[3] = pos + perspective.nearDist * (zAxis + xAxis + yAxis);

        mFrustum.verticies[4] = pos + perspective.farDist * (zAxis - xAxis - yAxis);
        mFrustum.verticies[5] = pos + perspective.farDist * (zAxis + xAxis - yAxis);
        mFrustum.verticies[6] = pos + perspective.farDist * (zAxis - xAxis + yAxis);
        mFrustum.verticies[7] = pos + perspective.farDist * (zAxis + xAxis + yAxis);
    }
    else
    {
        xAxis = matrix.GetRow(0);
        yAxis = matrix.GetRow(1);
        zAxis = matrix.GetRow(2);
        mScreenScale = Vector(1, 1, 0, 0);

        mFrustum.verticies[0] = pos + ortho.nearDist * zAxis + ortho.left * xAxis  + ortho.bottom *
                                yAxis;
        mFrustum.verticies[1] = pos + ortho.nearDist * zAxis + ortho.right * xAxis + ortho.bottom *
                                yAxis;
        mFrustum.verticies[2] = pos + ortho.nearDist * zAxis + ortho.left * xAxis  + ortho.top *
                                yAxis;
        mFrustum.verticies[3] = pos + ortho.nearDist * zAxis + ortho.right * xAxis + ortho.top *
                                yAxis;

        mFrustum.verticies[4] = pos + ortho.farDist * zAxis + ortho.left * xAxis   + ortho.bottom *
                                yAxis;
        mFrustum.verticies[5] = pos + ortho.farDist * zAxis + ortho.right * xAxis  + ortho.bottom *
                                yAxis;
        mFrustum.verticies[6] = pos + ortho.farDist * zAxis + ortho.left * xAxis   + ortho.top *
                                yAxis;
        mFrustum.verticies[7] = pos + ortho.farDist * zAxis + ortho.right * xAxis  + ortho.top *
                                yAxis;
    }

    mFrustum.CalculatePlanes();
}