C++ Matrix3::Inverse方法代码示例

void RotatePanorama::rotatePano(PanoramaData& panorama, const Matrix3& transformMat)
{
    for (unsigned int i = 0; i < panorama.getNrOfImages(); i++)
    {
        const SrcPanoImage & image = panorama.getImage(i);
        SrcPanoImage copy = image;
        double y = image.getYaw();
        double p = image.getPitch();
        double r = image.getRoll();
        Matrix3 mat;
        mat.SetRotationPT(DEG_TO_RAD(y), DEG_TO_RAD(p), DEG_TO_RAD(r));
        DEBUG_DEBUG("rotation matrix (PT) for img " << i << " << ypr:" << y << " " << p << " " << r << std::endl << mat);
        Matrix3 rotated;
        rotated = transformMat * mat;
        DEBUG_DEBUG("rotation matrix after transform: " << rotated);
        rotated.GetRotationPT(y,p,r);
        y = RAD_TO_DEG(y);
        p = RAD_TO_DEG(p);
        r = RAD_TO_DEG(r);
        DEBUG_DEBUG("rotated angles of img " << i << ": " << y << " " << p << " " << r);
        
        // Don't update a variable linked to a variable we already updated.
        conditional_set(Yaw, y);
        conditional_set(Pitch, p);
        conditional_set(Roll, r);
        if(image.getX()!=0.0 || image.getY()!=0.0 || image.getZ()!=0.0)
        {
            // rotate translation vector
            Vector3 vecRot=transformMat.Inverse().TransformVector(Vector3(image.getZ(), image.getX(), image.getY()));
            conditional_set(X, vecRot.y);
            conditional_set(Y, vecRot.z);
            conditional_set(Z, vecRot.x);
            // rotate translation plane
            mat.SetRotationPT(DEG_TO_RAD(image.getTranslationPlaneYaw()), DEG_TO_RAD(image.getTranslationPlanePitch()), 0.0);
            rotated = transformMat * mat;
            rotated.GetRotationPT(y,p,r);
            conditional_set(TranslationPlaneYaw, RAD_TO_DEG(y));
            conditional_set(TranslationPlanePitch, RAD_TO_DEG(p));
        };
        panorama.setImage(i, copy);
        panorama.imageChanged(i);
    }
}

void GLSLProgram<real>::SetupCamera()
{
	RenderState<real>* glState = RenderState<real>::Get();
	Camera<real>* camera = glState->GetCamera();
	Matrix4<float> projectionMatrix = camera->GetProjectionMatrix();
	Matrix4<float> viewMatrix = camera->GetGlobalToLocal();
	Matrix4<float> modelMatrix = glState->GetModelMatrix();
	Matrix4<float> modelViewProjectionMatrix = projectionMatrix * viewMatrix * modelMatrix;
	Matrix3<float> normalMatrix = modelMatrix.SubMatrix3( 0, 0 );
	normalMatrix.Inverse();
	normalMatrix.Transpose();

	// Model, view and projection matrices, camera position
	glUniformMatrix4fv( mModelMatrixLocation, 1, true, modelMatrix.GetArray() );
	glUniformMatrix4fv( mViewMatrixLocation, 1, true, viewMatrix.GetArray() );
	glUniformMatrix4fv( mProjectionMatrixLocation, 1, true, projectionMatrix.GetArray() );
	glUniformMatrix4fv( mModelViewProjectionMatrixLocation, 1, true, modelViewProjectionMatrix.GetArray() );
	glUniformMatrix3fv( mNormalMatrixLocation, 1, true, normalMatrix.GetArray() );
	Vector3<float> cameraPosition = camera->GetGlobalPosition();
	glUniform3fv( mCameraPositionLocation, 1, (float*)&cameraPosition );
}

void Road2::textureFit(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, int texfit, Vector2* texc, Vector3 pos, Vector3 lastpos, float width)
{
    int i;

    if (texfit == TEXFIT_BRICKWALL || texfit == TEXFIT_CONCRETEWALL || texfit == TEXFIT_CONCRETEWALLI)
    {
        Vector3 ps[4];
        ps[0] = p1;
        ps[1] = p2;
        ps[2] = p3;
        ps[3] = p4;
        Vector3 pref1 = pos;
        Vector3 pref2 = lastpos;
        //make matrix
        Vector3 bx = pref2 - pref1;
        bx.normalise();
        Vector3 by = Vector3::UNIT_Y;
        Vector3 bz = bx.crossProduct(by);
        //coordinates change matrix
        Matrix3 reverse;
        reverse.SetColumn(0, bx);
        reverse.SetColumn(1, by);
        reverse.SetColumn(2, bz);
        Matrix3 forward;
        forward = reverse.Inverse();
        //transpose
        for (i = 0; i < 4; i++)
        {
            Vector3 trv = forward * (ps[i] - pref1);
            if (texfit == TEXFIT_BRICKWALL)
            {
                float ty = 0.746 - trv.y * 0.25 / 4.5;
                // fix overlapping
                if (ty > 1)
                    ty = 1;
                texc[i] = Vector2(trv.x / 10.0, ty);
            }
            if (texfit == TEXFIT_CONCRETEWALL)
            {
                // fix overlapping
                float ty = 0.496 - (trv.y - 0.7) * 0.25 / 4.5;
                if (ty > 1)
                    ty = 1;
                texc[i] = Vector2(trv.x / 10.0, ty);
            }
            if (texfit == TEXFIT_CONCRETEWALLI)
            {
                float ty = 0.496 + trv.y * 0.25 / 4.5;
                // fix overlapping
                if (ty > 1)
                    ty = 1;
                texc[i] = Vector2(trv.x / 10.0, ty);
            }
        }
        return;
    }
    if (texfit == TEXFIT_ROAD || texfit == TEXFIT_ROADS1 || texfit == TEXFIT_ROADS2 || texfit == TEXFIT_ROADS3 || texfit == TEXFIT_ROADS4 || texfit == TEXFIT_CONCRETETOP || texfit == TEXFIT_CONCRETEUNDER)
    {
        Vector3 ps[4];
        ps[0] = p1;
        ps[1] = p2;
        ps[2] = p3;
        ps[3] = p4;
        Vector3 pref1 = pos;
        Vector3 pref2 = lastpos;
        //project
        for (i = 0; i < 4; i++)
            ps[i].y = 0;
        pref1.y = 0;
        pref2.y = 0;
        //make matrix
        Vector3 bx = pref2 - pref1;
        bx.normalise();
        Vector3 by = Vector3::UNIT_Y;
        Vector3 bz = bx.crossProduct(by);
        //coordinates change matrix
        Matrix3 reverse;
        reverse.SetColumn(0, bx);
        reverse.SetColumn(1, by);
        reverse.SetColumn(2, bz);
        Matrix3 forward;
        forward = reverse.Inverse();
        //transpose
        float trvrefz = 0.0;
        for (i = 0; i < 4; i++)
        {
            Vector3 trv = forward * (ps[i] - pref1);
            if (texfit == TEXFIT_CONCRETETOP)
            {
                if (i == 0)
                    trvrefz = trv.z;
                texc[i] = Vector2(trv.x / 10.0, 0.621 + (trv.z - trvrefz) * 0.25 / 4.5);
            }
            else
            {
                float v1 = 0.072;
                float v2 = 0.423;
                if (texfit == TEXFIT_ROADS1)
                {
                    v1 = 0.001;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			if (nodes[k].iswheel) continue;
			if (k==locs[i].ref) continue;
			if (k==locs[i].nx) continue;
			if (k==locs[i].ny) continue;
			Vector3 vt=nodes[k].smoothpos-nodes[locs[i].ref].smoothpos;
			vt.normalise();
			float cost=vx.dotProduct(vt);
			if (cost>0.707 || cost<-0.707) continue; //rejection, fails the orthogonality criterion (+-45 degree)
			cost=vy.dotProduct(vt);
			if (cost>0.707 || cost<-0.707) continue; //rejection, fails the orthogonality criterion (+-45 degree)
			float dist=(vertices[i]-nodes[k].smoothpos).length();
			if (dist<mindist) {mindist=dist;minnode=k;};
		}
		if (minnode==-1) LOG("FLEXBODY ERROR on mesh "+String(meshname)+": VZ node not found");
		locs[i].nz=minnode;

		//rright, check orientation
		Vector3 xyn=vx.crossProduct(vy);
		if (xyn.dotProduct(nodes[locs[i].nz].smoothpos-nodes[locs[i].ref].smoothpos)<0)
		{
			//the base is messed up
			int t=locs[i].nz;
			locs[i].nz=locs[i].ny;
			locs[i].ny=t;
		}
*/
		Vector3 vz=(nodes[locs[i].nx].smoothpos-nodes[locs[i].ref].smoothpos).crossProduct(nodes[locs[i].ny].smoothpos-nodes[locs[i].ref].smoothpos);
		vz.normalise();
		Matrix3 mat;
		mat.SetColumn(0, nodes[locs[i].nx].smoothpos-nodes[locs[i].ref].smoothpos);
		mat.SetColumn(1, nodes[locs[i].ny].smoothpos-nodes[locs[i].ref].smoothpos);
//		mat.SetColumn(2, nodes[locs[i].nz].smoothpos-nodes[locs[i].ref].smoothpos);
		mat.SetColumn(2, vz);
		mat=mat.Inverse();

		//compute coordinates in the newly formed euclidian basis
		locs[i].coords=mat*(vertices[i]-nodes[locs[i].ref].smoothpos);

		//thats it!
	}

	//shadow
	if (haveshadows)
	{
		LOG("FLEXBODY preparing for shadow volume");
		msh->prepareForShadowVolume(); //we do this always so we have only one datastructure format to manage
		msh->buildEdgeList();
	}

	//adjusting bounds
	AxisAlignedBox aab=msh->getBounds();
	Vector3 v=aab.getMinimum();
	float mi=v.x;
	if (v.y<mi) mi=v.y;
	if (v.z<mi) mi=v.z;
	mi=fabs(mi);
	v=aab.getMaximum();
	float ma=v.x;
	if (ma<v.y) ma=v.y;
	if (ma<v.z) ma=v.z;
	ma=fabs(ma);
	if (mi>ma) ma=mi;
	aab.setMinimum(Vector3(-ma,-ma,-ma));
	aab.setMaximum(Vector3(ma,ma,ma));
	msh->_setBounds(aab, true);

inline Vector3 Contact::CalculateFrictionImpulse(Matrix3 * inverseInertiaTensor) {
    Vector3 impulseContact;
    marb inverseMass = body[0]->GetInverseMass();

    // The equivalent of a cross product in matrices is multiplication
    // by a skew symmetric matrix - we build the matrix for converting
    // between linear and angular quantities.
    Matrix3 impulseToTorque;
    impulseToTorque.SetSkewSymmetric(relativeContactPosition[0]);

    // Build the matrix to convert contact impulse to change in velocity
    // in world coordinates.
    Matrix3 deltaVelWorld = impulseToTorque;
    deltaVelWorld *= inverseInertiaTensor[0];
    deltaVelWorld *= impulseToTorque;
    deltaVelWorld *= -1;

    // Check if we need to add body two's data
    if (body[1]) {
        // Set the cross product matrix
        impulseToTorque.SetSkewSymmetric(relativeContactPosition[1]);

        // Calculate the velocity change matrix
        Matrix3 deltaVelWorld2 = impulseToTorque;
        deltaVelWorld2 *= inverseInertiaTensor[1];
        deltaVelWorld2 *= impulseToTorque;
        deltaVelWorld2 *= -1;

        // Add to the total delta velocity.
        deltaVelWorld += deltaVelWorld2;

        // Add to the inverse mass
        inverseMass += body[1]->GetInverseMass();
    }

    // Do a change of basis to convert into contact coordinates.
    Matrix3 deltaVelocity = contactToWorld.Transpose();
    deltaVelocity *= deltaVelWorld;
    deltaVelocity *= contactToWorld;

    // Add in the linear velocity change
    deltaVelocity.data[0] += inverseMass;
    deltaVelocity.data[4] += inverseMass;
    deltaVelocity.data[8] += inverseMass;

    // Invert to get the impulse needed per unit velocity
    Matrix3 impulseMatrix = deltaVelocity.Inverse();

    // Find the target velocities to kill
    Vector3 velKill(desiredDeltaVelocity,
        -contactVelocity.y,
        -contactVelocity.z);

    // Find the impulse to kill target velocities
    impulseContact = impulseMatrix.Transform(velKill);

    // Check for exceeding friction
    marb planarImpulse = marb_sqrt(impulseContact.y*impulseContact.y + impulseContact.z*impulseContact.z);

    if (planarImpulse > impulseContact.x * friction) {
        // We need to use dynamic friction
        impulseContact.y /= planarImpulse;
        impulseContact.z /= planarImpulse;

        impulseContact.x = deltaVelocity.data[0] + deltaVelocity.data[1]*friction*impulseContact.y
												 + deltaVelocity.data[2]*friction*impulseContact.z;
        impulseContact.x = desiredDeltaVelocity / impulseContact.x;
        impulseContact.y *= friction * impulseContact.x;
        impulseContact.z *= friction * impulseContact.x;
    }
    return impulseContact;
}

// DIVISION A/B = A * (1/B) = A * Inverse(B)
Matrix3 Matrix3::operator/ ( const Matrix3& myMatrix ) const
{
	Matrix3 temp = myMatrix;

	return( *this * temp.Inverse() );
}