C++ Matrix4::get方法代码示例

/** Rotates the matrix according to a fictitious trackball, placed in
the middle of the given window.
The trackball is approximated by a Gaussian curve.
The trackball coordinate system is: x=right, y=up, z=to viewer<BR>
The origin of the mouse coordinates zero (0,0) is considered to be top left.
@param width, height  window size in pixels
@param fromX, fromY   mouse starting position in pixels
@param toX, toY       mouse end position in pixels
*/
Matrix4 Matrix4::trackballRotation(int width, int height, int fromX, int fromY, int toX, int toY)
{
	const float TRACKBALL_SIZE = 1.3f;              // virtual trackball size (empirical value)
	Matrix4 mInv;                                   // inverse of ObjectView matrix
	Vector3 v1, v2;                                 // mouse drag positions in normalized 3D space
	float smallSize;                                // smaller window size between width and height
	float halfWidth, halfHeight;                    // half window sizes
	float angle;                                    // rotational angle
	float d;                                        // distance

	// Compute mouse coordinates in window and normalized to -1..1
	// ((0,0)=window center, (-1,-1) = bottom left, (1,1) = top right)
	halfWidth = (float)width / 2.0f;
	halfHeight = (float)height / 2.0f;
	smallSize = (halfWidth < halfHeight) ? halfWidth : halfHeight;
	v1.set(((float)fromX - halfWidth) / smallSize, ((float)(height - fromY) - halfHeight) / smallSize, 0);
	v2.set(((float)toX - halfWidth) / smallSize, ((float)(height - toY) - halfHeight) / smallSize, 0);

	// Compute z-coordinates on Gaussian trackball:
	d = sqrtf(v1[0] * v1[0] + v1[1] * v1[1]);
	v1.set(v1[0], v1[1], expf(-TRACKBALL_SIZE * d * d));

	d = sqrtf(v2[0] * v2[0] + v2[1] * v2[1]);
	v2.set(v2[0], v2[1], expf(-TRACKBALL_SIZE * d * d));

	// Compute rotational angle:
	double temp2;
	temp2 = v1.dot(v2);
	temp2 /= (v1.magnitude()*v2.magnitude());
	angle = acos(temp2);                          // angle = angle between v1 and v2

	// Compute rotational axis:
	v2.cross(v2, v1);                                  // v2 = v2 x v1 (cross product)

	// Convert axis coordinates (v2) from WCS to OCS:
	mInv.identity();
	mInv.set(0, 0, m[0][0]);
	mInv.set(0, 1, m[0][1]);
	mInv.set(0, 2, m[0][2]);
	mInv.set(1, 0, m[1][0]);
	mInv.set(1, 1, m[1][1]);
	mInv.set(1, 2, m[1][2]);
	mInv.set(2, 0, m[2][0]);
	mInv.set(2, 1, m[2][1]);
	mInv.set(2, 2, m[2][2]);

	mInv.transpose();                             // invert orthogonal matrix mInv
	int temp3[3] = { 0 };                            // v2 = v2 x mInv (matrix multiplication)
	for (int i = 0; i < 3; i++){
		temp3[i] = v2[0] * mInv.get(0, i) * v2[1] * mInv.get(1, i) * v2[2] * mInv.get(2, i);
	}
	v2.set(temp3[0], temp3[1], temp3[2]);
	v2.normalize();                                 // normalize v2 before rotation

	// Perform acutal model view matrix modification:
	return rotateaxis(-angle, v2);      // rotate model view matrix
}

bool Rectangle::isInBoundingSphere(Matrix4 m)
{
    Vector3 p = Vector3(m.get(0,3), m.get(1,3), m.get(2,3));
    double myRadius = max(fabs(width/float(2)), 
                      max(fabs(base/float(2)), fabs(height/(float(2)))));
    for (int i = 0; i < 6; ++i)
    {
        Vector3 n(planes[i][0], planes[i][1], planes[i][2]);
        if (n.dot(p) + planes[i][3] <= -myRadius)
            return false;
    }
    return true;
}

bool MathTestBench::test_m4_op_star_m4(void)
{
    bool pass = 0x0;
    
    Matrix4 a(1,0,0,0,0,2,0,0,0,0,3,0,0,0,0,1);
    Matrix4 b(5,0,0,0,0,6,0,0,0,0,7,0,0,0,0,1);
    
    Matrix4 c = a * b;
    
    pass = approx(c.get(0, 0), 5) && approx(c.get(1, 1), 12) && approx(c.get(2, 2), 21) && approx(c.get(3, 3), 1);
    
    printTestLine("Matrix4.(*)(Matrix4)", pass);
    return pass;
}

void displayCB()
{
    // clear buffer
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

    // save the initial ModelView matrix before modifying ModelView matrix
    glPushMatrix();

    // tramsform camera
    matrixView.identity();
    matrixView.rotate(cameraAngleY, 0, 1, 0);
    matrixView.rotate(cameraAngleX, 1, 0, 0);
    matrixView.translate(0, 0, -cameraDistance);
    //@@ the equivalent code for using OpenGL routine is:
    //@@ glTranslatef(0, 0, -cameraDistance);
    //@@ glRotatef(cameraAngleX, 1, 0, 0);   // pitch
    //@@ glRotatef(cameraAngleY, 0, 1, 0);   // heading

    // copy view matrix to OpenGL
    glLoadMatrixf(matrixView.get());

    drawGrid();                         // draw XZ-grid with default size

    // compute model matrix
    matrixModel.identity();
    //matrixModel.rotateZ(45);        // rotate 45 degree on Z-axis
    matrixModel.rotateY(45);        // rotate 45 degree on Y-axis
    matrixModel.translate(0, 1, 0); // move 2 unit up

    // compute modelview matrix
    matrixModelView = matrixView * matrixModel;

    // copy modelview matrix to OpenGL
    glLoadMatrixf(matrixModelView.get());

    drawAxis();
    drawModel();

    // draw info messages
    showInfo();

    glPopMatrix();

    glutSwapBuffers();
}

void Matrix4::mul(const Matrix4 &a,const Matrix4 &b) {
    if ((this==&a) || (this==&b)) error("auto mul : use mul(const Matrix4 &)",__LINE__,__FILE__);
    for(int i=0;i<4;i++) {
        for(int j=0;j<4;j++) {
            int tt=i+j*4;
            this->set(tt,0);
            for(int k=0;k<4;k++) {
                this->set(tt, (*this)[tt]+a[i+k*4]*b.get(k+j*4));
            }
        }
    }
}

void Matrix4::loadMultiply(const Matrix4& u, const Matrix4& v) {
    for (int i = 0 ; i < 4 ; i++) {
        float x = 0;
        float y = 0;
        float z = 0;
        float w = 0;

        for (int j = 0 ; j < 4 ; j++) {
            const float e = v.get(i, j);
            x += u.get(j, 0) * e;
            y += u.get(j, 1) * e;
            z += u.get(j, 2) * e;
            w += u.get(j, 3) * e;
        }

        set(i, 0, x);
        set(i, 1, y);
        set(i, 2, z);
        set(i, 3, w);
    }

    mType = kTypeUnknown;
}

//--------------------------------------------------------------
ofMatrix4x4 ofxOpenVR::getCurrentViewProjectionMatrix(vr::Hmd_Eye nEye)
{
	Matrix4 matMVP;
	if (nEye == vr::Eye_Left)
	{
		matMVP = _mat4ProjectionLeft * _mat4eyePosLeft * _mat4HMDPose;
	}
	else if (nEye == vr::Eye_Right)
	{
		matMVP = _mat4ProjectionRight * _mat4eyePosRight *  _mat4HMDPose;
	}

	ofMatrix4x4 matrix(matMVP.get());
	return matrix;
}

//--------------------------------------------------------------
ofMatrix4x4 ofxOpenVR::getCurrentViewMatrix(vr::Hmd_Eye nEye)
{
	Matrix4 matV;
	if (nEye == vr::Eye_Left)
	{
		matV = _mat4eyePosLeft * _mat4HMDPose;
	}
	else if (nEye == vr::Eye_Right)
	{
		matV = _mat4eyePosRight *  _mat4HMDPose;
	}

	ofMatrix4x4 matrix(matV.get());
	return matrix;
}

//--------------------------------------------------------------
ofMatrix4x4 ofxOpenVR::getCurrentProjectionMatrix(vr::Hmd_Eye nEye)
{
	Matrix4 matP;
	if (nEye == vr::Eye_Left)
	{
		matP = _mat4ProjectionLeft;
	}
	else if (nEye == vr::Eye_Right)
	{
		matP = _mat4ProjectionRight;
	}

	ofMatrix4x4 matrix(matP.get());
	return matrix;
}

Matrix4 Matrix4::multiply(Matrix4 a)
{
    Matrix4 b;
    float solutions [4][4];

    //Implement a more performant Matrix * Matrix multiplication

    //The current implementation below is not very efficient:
    //It allocates an additional 8 vectors on the stack and calls their constructors
    //It also calls off to additional functions (which create even more vectors!)
    //Which results in a lot of time being wasted on memory operations
    //This is bad, really bad, ultra bad D:

    //Hint: Loops!
    //Hint for the ambitious: SIMD!
    /*
    Vector4 row1(m[0][0], m[1][0], m[2][0], m[3][0]);
    Vector4 row2(m[0][1], m[1][1], m[2][1], m[3][1]);
    Vector4 row3(m[0][2], m[1][2], m[2][2], m[3][2]);
    Vector4 row4(m[0][3], m[1][3], m[2][3], m[3][3]);

    Vector4 col1(a.m[0][0], a.m[0][1], a.m[0][2], a.m[0][3]);
    Vector4 col2(a.m[1][0], a.m[1][1], a.m[1][2], a.m[1][3]);
    Vector4 col3(a.m[2][0], a.m[2][1], a.m[2][2], a.m[2][3]);
    Vector4 col4(a.m[3][0], a.m[3][1], a.m[3][2], a.m[3][3]);

    b.set(row1.dot(col1), row2.dot(col1), row3.dot(col1), row4.dot(col1),
             row1.dot(col2), row2.dot(col2), row3.dot(col2), row4.dot(col2),
             row1.dot(col3), row2.dot(col3), row3.dot(col3), row4.dot(col3),
             row1.dot(col4), row2.dot(col4), row3.dot(col4), row4.dot(col4) );
    */

    for (int r = 0; r < 4; r++) {
        for (int c = 0; c < 4; c++) {
            solutions[c][r] = 0;
            for (int i = 0; i < 4; i++) {
                solutions[c][r] += m[i][r]  * a.get(c, i);
            }
        }
    }
    b.set(solutions[0][0], solutions[0][1], solutions[0][2], solutions[0][3],
          solutions[1][0], solutions[1][1], solutions[1][2], solutions[1][3],
          solutions[2][0], solutions[2][1], solutions[2][2], solutions[2][3],
          solutions[3][0], solutions[3][1], solutions[3][2], solutions[3][3]);
    return b;
}

void Matrix4::scale(double a, double b, double c)
{
	Matrix4 n = Matrix4(
		a, 0, 0, 0,
		0, b, 0, 0,
		0, 0, c, 0,
		0, 0, 0, 1
	);

	Matrix4 r = this->multiply(n);
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] = r.get(i, j);
		}
	}
}

void reshapeCB(int w, int h)
{
    screenWidth = w;
    screenHeight = h;

    // set viewport to be the entire window
    glViewport(0, 0, (GLsizei)w, (GLsizei)h);

    // set perspective viewing frustum
    glMatrixMode(GL_PROJECTION);
    matrixProjection = setFrustum(45, (float)w/h, 1.0f, 100.0f);
    glLoadMatrixf(matrixProjection.get());
    //@@ the equivalent OpenGL call
    //@@ gluPerspective(45.0f, (float)(w)/h, 1.0f, 100.0f); // FOV, AspectRatio, NearClip, FarClip

    // DEBUG
    std::cout << "===== Projection Matrix =====\n";
    std::cout << matrixProjection << std::endl;

    // switch to modelview matrix in order to set scene
    glMatrixMode(GL_MODELVIEW);
}

 /// test equaility of two matrices
 void ASSERT_MATRIX_EQ(const Matrix4& m1, const Matrix4& m2)
 {
     ASSERT_FLOAT_EQ(m1.get(0,0), m2.get(0,0));
     ASSERT_FLOAT_EQ(m1.get(0,1), m2.get(0,1));
     ASSERT_FLOAT_EQ(m1.get(0,2), m2.get(0,2));
     ASSERT_FLOAT_EQ(m1.get(0,3), m2.get(0,3));
     ASSERT_FLOAT_EQ(m1.get(1,0), m2.get(1,0));
     ASSERT_FLOAT_EQ(m1.get(1,1), m2.get(1,1));
     ASSERT_FLOAT_EQ(m1.get(1,2), m2.get(1,2));
     ASSERT_FLOAT_EQ(m1.get(1,3), m2.get(1,3));
     ASSERT_FLOAT_EQ(m1.get(2,0), m2.get(2,0));
     ASSERT_FLOAT_EQ(m1.get(2,1), m2.get(2,1));
     ASSERT_FLOAT_EQ(m1.get(2,2), m2.get(2,2));
     ASSERT_FLOAT_EQ(m1.get(2,3), m2.get(2,3));
     ASSERT_FLOAT_EQ(m1.get(3,0), m2.get(3,0));
     ASSERT_FLOAT_EQ(m1.get(3,1), m2.get(3,1));
     ASSERT_FLOAT_EQ(m1.get(3,2), m2.get(3,2));
     ASSERT_FLOAT_EQ(m1.get(3,3), m2.get(3,3));
 }

void onDisplay(void) 
{
	while(isModelLoading == 1){
		//printf("waiting\n");
	}

	glClearColor(0.9f, 0.5f, 0.6f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);


	Matrix4 M;
	// aMVP = identity() * M_normalize(Tn*Sn) * M_transformation(S or R or T) * V(lookat) * P(projection)
			
	M.identity();
			
	// normalize
	M = M * normalization_matrix;

	// previous rotate matrix
	M = M * preM;

	// mouse rotate
	if(isRBtnDown && isMouseMoving){
		float temp = sqrt(rotateX * rotateX + rotateY * rotateY);
		if(temp != 0){
			Matrix4 newM = myRotateMatrix(-temp / 4.0f, rotateY/temp, rotateX/temp, 0.0f);
			// divided by 4 in order to rotate slower
			M = M * newM;
			preM = preM * newM;
		}
	}
	
	// auto rotate
	if (isAutoRotating) {
		Matrix4 newM = myRotateMatrix(3.0f, 0.0f, 1.0f, 0.0f);
		M = M * newM;
		preM = preM * newM;
	}
	
	// scale
	M = M * myScaleMatrix(scale, scale, scale);

	// mouse translate
	M = M * myTranslateMatrix(preTranslateX, preTranslateY, 0);
	if(isMouseMoving && isLBtnDown){
		float ratio = (float)screenWidth / (float)screenHeight;
		float alphaX = (xmax - xmin) / (float)screenWidth;
		float alphaY = (ymax - ymin) / (float)screenHeight;
		if(ratio >= 1){
			alphaX *= ratio;
		}else{
			alphaY /= ratio;
		}
		M = M * myTranslateMatrix(translateX*alphaX, translateY*alphaY, 0);
		preTranslateX += translateX * alphaX;
		preTranslateY += translateY * alphaY;
	}

	Matrix4 modelMatrix;
	// Model matrix ends here
	for(int i=0;i<16;i++){
		modelMatrix[i] = M[i];
	}

	// V (lookat)
	M = M * myLookAtMatrix(eyeVec[0],eyeVec[1],eyeVec[2],  eyeVec[0],eyeVec[1],eyeVec[2]-1.0f,  0,1,0);

	// P (project)
	if(projectionMode == PROJECTION_PARA)
	{
		M = M * parallelProjectionMatrix;
	}
	else if(projectionMode == PROJECTION_PERS)
	{
		M = M * perspectiveProjectionMatrix;
	}
	else 
	{
		fprintf(stderr, "ERROR!\n");
		system("pause");
		exit(-1);
	}

	// V (view port)
	M = M * viewPort;

	for(int i=0;i<16;i++) aMVP[i] = M[i];

	// Matrices
	glUniformMatrix4fv(iLocMVP, 1, GL_FALSE, M.get());
	glUniformMatrix4fv(iLocModelMatrix, 1, GL_FALSE, modelMatrix.get());

	glUniform1i(iLocTexMap, MAPPING);

	GLMgroup* group = OBJ->groups;
	int gCount = 0;
	while(group){
		// enable attributes array
		glEnableVertexAttribArray(iLocPosition);
		// TODO: texture VertexAttribArray is enabled here
//.........这里部分代码省略.........

Matrix4 Matrix4::operator*(const Matrix4& rhs) {
	Matrix4 result;
	result.m[0][0] = this->get(0, 0) * rhs.get(0, 0) + this->get(0, 1) * rhs.get(1, 0) + this->get(0, 2) * rhs.get(2, 0) + this->get(0, 3) * rhs.get(3, 0);
	result.m[0][1] = this->get(0, 0) * rhs.get(0, 1) + this->get(0, 1) * rhs.get(1, 1) + this->get(0, 2) * rhs.get(2, 1) + this->get(0, 3) * rhs.get(3, 1);
	result.m[0][2] = this->get(0, 0) * rhs.get(0, 2) + this->get(0, 1) * rhs.get(1, 2) + this->get(0, 2) * rhs.get(2, 2) + this->get(0, 3) * rhs.get(3, 2);
	result.m[0][3] = this->get(0, 0) * rhs.get(0, 3) + this->get(0, 1) * rhs.get(1, 3) + this->get(0, 2) * rhs.get(2, 3) + this->get(0, 3) * rhs.get(3, 3);
	result.m[1][0] = this->get(1, 0) * rhs.get(0, 0) + this->get(1, 1) * rhs.get(1, 0) + this->get(1, 2) * rhs.get(2, 0) + this->get(1, 3) * rhs.get(3, 0);
	result.m[1][1] = this->get(1, 0) * rhs.get(0, 1) + this->get(1, 1) * rhs.get(1, 1) + this->get(1, 2) * rhs.get(2, 1) + this->get(1, 3) * rhs.get(3, 1);
	result.m[1][2] = this->get(1, 0) * rhs.get(0, 2) + this->get(1, 1) * rhs.get(1, 2) + this->get(1, 2) * rhs.get(2, 2) + this->get(1, 3) * rhs.get(3, 2);
	result.m[1][3] = this->get(1, 0) * rhs.get(0, 3) + this->get(1, 1) * rhs.get(1, 3) + this->get(1, 2) * rhs.get(2, 3) + this->get(1, 3) * rhs.get(3, 3);
	result.m[2][0] = this->get(2, 0) * rhs.get(0, 0) + this->get(2, 1) * rhs.get(1, 0) + this->get(2, 2) * rhs.get(2, 0) + this->get(2, 3) * rhs.get(3, 0);
	result.m[2][1] = this->get(2, 0) * rhs.get(0, 1) + this->get(2, 1) * rhs.get(1, 1) + this->get(2, 2) * rhs.get(2, 1) + this->get(2, 3) * rhs.get(3, 1);
	result.m[2][2] = this->get(2, 0) * rhs.get(0, 2) + this->get(2, 1) * rhs.get(1, 2) + this->get(2, 2) * rhs.get(2, 2) + this->get(2, 3) * rhs.get(3, 2);
	result.m[2][3] = this->get(2, 0) * rhs.get(0, 3) + this->get(2, 1) * rhs.get(1, 3) + this->get(2, 2) * rhs.get(2, 3) + this->get(2, 3) * rhs.get(3, 3);
	result.m[3][0] = this->get(3, 0) * rhs.get(0, 0) + this->get(3, 1) * rhs.get(1, 0) + this->get(3, 2) * rhs.get(2, 0) + this->get(3, 3) * rhs.get(3, 0);
	result.m[3][1] = this->get(3, 0) * rhs.get(0, 1) + this->get(3, 1) * rhs.get(1, 1) + this->get(3, 2) * rhs.get(2, 1) + this->get(3, 3) * rhs.get(3, 1);
	result.m[3][2] = this->get(3, 0) * rhs.get(0, 2) + this->get(3, 1) * rhs.get(1, 2) + this->get(3, 2) * rhs.get(2, 2) + this->get(3, 3) * rhs.get(3, 2);
	result.m[3][3] = this->get(3, 0) * rhs.get(0, 3) + this->get(3, 1) * rhs.get(1, 3) + this->get(3, 2) * rhs.get(2, 3) + this->get(3, 3) * rhs.get(3, 3);


	return result.transpose();
}