C++ Matrix3f::transposed方法代码示例

void AP_AHRS_NavEKF::update_SITL(void)
{
    if (_sitl == nullptr) {
        _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
        if (_sitl == nullptr) {
            return;
        }
    }

    const struct SITL::sitl_fdm &fdm = _sitl->state;

    if (active_EKF_type() == EKF_TYPE_SITL) {
        roll  = radians(fdm.rollDeg);
        pitch = radians(fdm.pitchDeg);
        yaw   = radians(fdm.yawDeg);

        fdm.quaternion.rotation_matrix(_dcm_matrix);

        update_cd_values();
        update_trig();

        _gyro_drift.zero();

        _gyro_estimate = Vector3f(radians(fdm.rollRate),
                                  radians(fdm.pitchRate),
                                  radians(fdm.yawRate));

        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
            const Vector3f accel(fdm.xAccel,
                           fdm.yAccel,
                           fdm.zAccel);
            _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
        }
        _accel_ef_ekf_blended = _accel_ef_ekf[0];

    }

    if (_sitl->odom_enable) {
        // use SITL states to write body frame odometry data at 20Hz
        uint32_t timeStamp_ms = AP_HAL::millis();
        if (timeStamp_ms - _last_body_odm_update_ms > 50) {
            const float quality = 100.0f;
            const Vector3f posOffset(0.0f, 0.0f, 0.0f);
            const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
            _last_body_odm_update_ms = timeStamp_ms;
            timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
            Vector3f delAng(radians(fdm.rollRate),
                                       radians(fdm.pitchRate),
                                       radians(fdm.yawRate));
            delAng *= delTime;
            // rotate earth velocity into body frame and calculate delta position
            Matrix3f Tbn;
            Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
            const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
            const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
            // write to EKF
            EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
        }
    }
}

void
Compass::null_offsets(const Matrix3f &dcm_matrix)
{
    // Update our estimate of the offsets in the magnetometer
    Vector3f    calc(0.0, 0.0, 0.0);        // XXX should be safe to remove explicit init here as the default ctor should do the right thing
    Matrix3f    dcm_new_from_last;
    float       weight;

    Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);

    if(_null_init_done) {
        dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix;      // Note 11/20/2010: transpose() is not working, transposed() is.

        weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z);
        if (weight > .001) {
            calc = mag_body_new + _mag_body_last;                // Eq 11 from Bill P's paper
            calc -= dcm_new_from_last * _mag_body_last;
            calc -= dcm_new_from_last.transposed() * mag_body_new;
            if(weight > 0.5) weight = 0.5;
            calc = calc * (weight);
            _offset.set(_offset.get() - calc);
        }
    } else {
        _null_init_done = true;
    }
    _mag_body_last = mag_body_new - calc;
    _last_dcm_matrix = dcm_matrix;

}

Surface makeSurfRev(const Curve &profile, unsigned steps)
{
    Surface surface;
    vector<Vector3f> initialVV;
    
    if (!checkFlat(profile))
    {
        cerr << "surfRev profile curve must be flat on xy plane." << endl;
        exit(0);
    }

    // TODO: Here you should build the surface.  See surf.h for details.

    //cerr << "\t>>> makeSurfRev called (but not implemented).\n\t>>> Returning empty surface." << endl;
 

    for (unsigned i=0; i<profile.size(); i++){

        for (unsigned j=0; j<=steps; j++){
            float t = 2.0f * M_PI * float( j ) / steps;

	    //Matrix operations used to calulate normal
	    Matrix4f rotatM = Matrix4f::rotateY(t);
	    Matrix3f rotatMsub = rotatM.getSubmatrix3x3(0,0);
	    Matrix3f rotatMtrans = rotatMsub.transposed();
	    Matrix3f rotatN = rotatMtrans.inverse();

	    //Calculate surface vertex
	    Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
	    Vector4f surfaceVecInit = rotatM*surfaceCalc;
	    Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);

	    //Calculate surface normal
	    Vector3f surfaceVNInit = rotatN*profile[i].N;

	    //Push vectors into surface data
	    surface.VV.push_back(surfaceVec);
	    surface.VN.push_back(-1*surfaceVNInit);

        }
    }

    //Calculate faces once all the vertices are added
    for (unsigned k=0; k<surface.VV.size()-(steps+1);k++){
	Tup3u firstTri;		//faces uses a series of connected triangles
	Tup3u secondTri;

	if ((k+1)%(steps+1) != 0)	//Create triangles (considering edge conditions)
	{
	    //Triangles in counter-clockwise manner
	    firstTri = Tup3u(k+1, k, k+steps+1);
            secondTri = Tup3u(k+1, k+1+steps, k+2+steps);   
        }

	surface.VF.push_back(firstTri);
	surface.VF.push_back(secondTri);  
    }

    return surface;
}

/*
  given a magnetic heading, and roll, pitch, yaw values,
  calculate consistent magnetometer components

  All angles are in radians
 */
static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{
	Vector3f Bearth, m;
	Matrix3f R;

	// Bearth is the magnetic field in Canberra. We need to adjust
	// it for inclination and declination
	Bearth(MAG_FIELD_STRENGTH, 0, 0);
	R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
	Bearth = R * Bearth;

	// create a rotation matrix for the given attitude
	R.from_euler(roll, pitch, yaw);

	// convert the earth frame magnetic vector to body frame, and
	// apply the offsets
	m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
	return m + (rand_vec3f() * sitl.mag_noise);
}

void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
{
    get_rotation_vehicle_to_reference(m);
    m = m.transposed();
}

void QD3D11MultiViewportViewer::mouseMoveEvent( QMouseEvent* event )
{
	Vector2i curPos( event->x(), event->y() );
	Vector2f delta = curPos - m_prevPos;

#if 1

	float pitchSpeed = m_flipMouseUpDown ? m_mousePitchSpeed : -m_mousePitchSpeed;
	const float yawSpeed = 0.005f;
	const float panSpeed = 0.005f;
	const float walkSpeed = -0.005f;

	Matrix3f worldToCamera = m_perspectiveCamera.viewMatrix().getSubmatrix3x3( 0, 0 );
	Matrix3f cameraToWorld = m_perspectiveCamera.inverseViewMatrix().getSubmatrix3x3( 0, 0 );
	
	Vector3f eye = m_perspectiveCamera.eye();
	Vector3f x = m_perspectiveCamera.right();
	Vector3f y = m_perspectiveCamera.up();
	Vector3f z = m_perspectiveCamera.forward();

	// rotate
	if( event->buttons() == Qt::LeftButton )
	{
		// pitch around the local x axis		
		float pitch = pitchSpeed * delta.y;

		Matrix3f pitchMatrix = Matrix3f::rotateX( pitch );		

		y = cameraToWorld * pitchMatrix * worldToCamera * y;
		z = cameraToWorld * pitchMatrix * worldToCamera * z;

		// yaw around the world up vector
		float yaw = yawSpeed * delta.x;

		Matrix3f yawMatrix = m_groundPlaneToWorld * Matrix3f::rotateY( yaw ) * m_worldToGroundPlane;

		x = yawMatrix * x;
		y = yawMatrix * y;
		z = yawMatrix * z;		

		m_perspectiveCamera.setLookAt( eye, eye + z, y );
	}
	// walk
	else if( event->buttons() == Qt::RightButton )
	{
		float dx = panSpeed * delta.x;
		float dz = walkSpeed * delta.y;

		translate( dx, 0, dz );
	}
	// move up/down
	else if( event->buttons() == Qt::MiddleButton )
	{
		float dy = -panSpeed * delta.y;
		translate( 0, dy, 0 );
	}

#else

	if(event->buttons() & Qt::RightButton) //rotate
	{
		float rotSpeed = 0.005f; //radians per pixel
		Quat4f rotation;
		rotation.setAxisAngle(rotSpeed * delta.abs(), Vector3f(-delta[1], -delta[0], 0));
		Matrix3f rotMatrix = Matrix3f::rotation(rotation);
		Matrix3f viewMatrix = m_camera.getViewMatrix().getSubmatrix3x3(0, 0);
		rotMatrix = viewMatrix.transposed() * rotMatrix * viewMatrix;

		Vector3f eye, center, up;
		m_camera.getLookAt(&eye, &center, &up);
		m_camera.setLookAt(center + rotMatrix * (eye - center), center, rotMatrix * up);
	}
	else if(event->buttons() & Qt::LeftButton) //translate
	{
		float speed = 10.f;
		Vector3f screenDelta(delta[0], delta[1], 0);
		screenDelta[0] /= -double(width());
		screenDelta[1] /= double(height());
		Matrix4f iViewProjMatrix = m_camera.getInverseViewProjectionMatrix();
		Vector3f worldDelta = iViewProjMatrix.getSubmatrix3x3(0, 0) * (speed * screenDelta);

		Vector3f eye, center, up;
		m_camera.getLookAt(&eye, &center, &up);
		m_camera.setLookAt(eye + worldDelta, center + worldDelta, up);
	}
#endif

	m_prevPos = curPos;
	update();
}

Surface makeGenCyl(const Curve &profile, const Curve &sweep )
{
    Surface surface;

    if (!checkFlat(profile))
    {
        cerr << "genCyl profile curve must be flat on xy plane." << endl;
        exit(0);
    }

    // TODO: Here you should build the surface.  See surf.h for details.

    //cerr << "\t>>> makeGenCyl called (but not implemented).\n\t>>> Returning empty surface." <<endl;
    
    for (unsigned i=0; i<profile.size(); i++){

        for (unsigned j=0; j<sweep.size(); j++){

	    //Matrix from sweep
            Matrix4f coordM(sweep[j].N[0], sweep[j].B[0], sweep[j].T[0], sweep[j].V[0], 
			    sweep[j].N[1], sweep[j].B[1], sweep[j].T[1], sweep[j].V[1], 
			    sweep[j].N[2], sweep[j].B[2], sweep[j].T[2], sweep[j].V[2], 
			    0.f, 0.f, 0.f, 1.f);

	    //Matrix operations to get normal
	    Matrix3f rotatMsub = coordM.getSubmatrix3x3(0,0);
	    Matrix3f rotatMtrans = rotatMsub.transposed();
	    Matrix3f rotatN = rotatMtrans.inverse();

	    //Calculate surface vertex
	    Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
	    Vector4f surfaceVecInit = coordM*surfaceCalc;
	    Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);

	    //Calculate surface normal
	    Vector3f surfaceVNInit = rotatN*profile[i].N;

	    //Push vectors into surface data
	    surface.VV.push_back(surfaceVec);
	    surface.VN.push_back(-1*surfaceVNInit);
        }
    }

    //Calculate faces once all the vertices are added
    for (unsigned k=0; k<surface.VV.size()-(sweep.size());k++){

	Tup3u firstTri;		//faces uses a series of connected triangles
	Tup3u secondTri;

	if ((k+1)%(sweep.size()) != 0)	//Create triangles (considering edge conditions)
	{
	    //Triangles in counter-clockwise manner
	    firstTri = Tup3u(k+1, k, k+sweep.size());
            secondTri = Tup3u(k+1, k+sweep.size(), k+1+sweep.size());   
        }

	surface.VF.push_back(firstTri);
	surface.VF.push_back(secondTri);
            
    }

    return surface;
}