C++ Matrix4d::getAngles方法代码示例

//.........这里部分代码省略.........
    else
    {
        char buf[255];
        fgets( buf, 255, fid );  // Advance past "...BEM Propeller..."

        fscanf( fid, "Num_Sections: %d\n", &num_sect );
        fscanf( fid, "Num_Blade: %d\n", &num_blade );
        fscanf( fid, "Diameter: %lf\n", &diam );
        fscanf( fid, "Beta 3/4 (deg): %lf\n", &beta34 );
        fscanf( fid, "Feather (deg): %lf\n", &feather );
        double x, y, z;
        fscanf( fid, "Center: %lf, %lf, %lf\n", &x, &y, &z );
        cen.set_xyz( x, y, z );
        fscanf( fid, "Normal: %lf, %lf, %lf\n", &x, &y, &z );
        norm.set_xyz( x, y, z );

        r_vec.resize( num_sect );
        chord_vec.resize( num_sect );
        twist_vec.resize( num_sect );
        rake_vec.resize( num_sect );
        skew_vec.resize( num_sect );

        fgets( buf, 255, fid );  // Advance past "Radius/R, Chord/R, Twist (deg), Rake/R, Skew/R"

        for ( int i = 0; i < num_sect; i++ )
        {
            fscanf( fid, "%lf, %lf, %lf, %lf, %lf\n", &r_vec[i], &chord_vec[i], &twist_vec[i], &rake_vec[i], &skew_vec[i] );
        }

        fclose( fid );
    }

    if ( true )
    {
        printf( "Num_Sections: %d\n", num_sect );
        printf( "Num_Blade: %d\n", num_blade );
        printf( "Diameter: %.8f\n", diam );
        printf( "Beta 3/4 (deg): %.8f\n", beta34 );
        printf( "Feather (deg): %.8f\n", feather );
        printf( "Center: %.8f, %.8f, %.8f\n", cen.x(), cen.y(), cen.z() );
        printf( "Normal: %.8f, %.8f, %.8f\n", norm.x(), norm.y(), norm.z() );

        printf( "\nRadius/R, Chord/R, Twist (deg), Rake/R, Skew/R\n" );
        for ( int i = 0; i < num_sect; i++ )
        {
            printf( "%.8f, %.8f, %.8f, %.8f, %.8f\n", r_vec[i], chord_vec[i], twist_vec[i], rake_vec[i], skew_vec[i] );
        }
    }

    double rfirst = r_vec[ 0 ];
    double rlast = r_vec[ r_vec.size() - 1 ];

    int nxsec = m_XSecSurf.NumXSec();

    PropXSec* xs;
    xs = ( PropXSec* ) m_XSecSurf.FindXSec( 0 );
    if ( xs )
    {
        xs->m_RadiusFrac = rfirst;
    }

    xs = ( PropXSec* ) m_XSecSurf.FindXSec( nxsec - 1 );
    if ( xs )
    {
        xs->m_RadiusFrac = rlast;
    }

    m_Diameter = diam;
    m_Nblade = num_blade;
    m_Beta34 = beta34;
    m_Feather = feather;

    m_XRelLoc = cen.x();
    m_YRelLoc = cen.y();
    m_ZRelLoc = cen.z();

    norm = norm * -1.0;
    norm.normalize();
    vec3d minor;
    minor.v[ norm.minor_comp() ] = 1.0;
    vec3d counter = cross( norm, minor );
    counter.normalize();
    minor = cross( counter, norm );
    minor.normalize();

    Matrix4d mat;
    mat.setBasis( norm, minor, counter );

    vec3d angles = mat.getAngles();
    m_XRelRot = angles.x();
    m_YRelRot = angles.y();
    m_ZRelRot = angles.z();

    m_ChordCurve.SetCurve( r_vec, chord_vec, vsp::PCHIP );
    m_TwistCurve.SetCurve( r_vec, twist_vec, vsp::PCHIP );
    m_RakeCurve.SetCurve( r_vec, rake_vec, vsp::PCHIP );
    m_SkewCurve.SetCurve( r_vec, skew_vec, vsp::PCHIP );

    return 1;
}

void StackGeom::EnforceOrder( StackXSec* xs, int indx, int policy )
{
    int nxsec = m_XSecSurf.NumXSec();

    bool first = false;
    bool last = false;
    bool nextlast = false;

    if( indx == 0 ) first = true;
    else if( indx == (nxsec-1) ) last = true;
    else if( indx == (nxsec-2) ) nextlast = true;

    // STACK_FREE implicit.
    if ( first )
    {
        xs->m_XDelta.SetLowerUpperLimits( 0.0, 0.0 );
        xs->m_YDelta.SetLowerUpperLimits( 0.0, 0.0 );
        xs->m_ZDelta.SetLowerUpperLimits( 0.0, 0.0 );

        xs->m_XRotate.SetLowerUpperLimits( 0.0, 0.0 );
        xs->m_YRotate.SetLowerUpperLimits( 0.0, 0.0 );
        xs->m_ZRotate.SetLowerUpperLimits( 0.0, 0.0 );
    }
    else
    {
        xs->m_XDelta.SetLowerUpperLimits( -1.0e12, 1.0e12 );
        xs->m_YDelta.SetLowerUpperLimits( -1.0e12, 1.0e12 );
        xs->m_ZDelta.SetLowerUpperLimits( -1.0e12, 1.0e12 );

        xs->m_XRotate.SetLowerUpperLimits( -180.0, 180.0 );
        xs->m_YRotate.SetLowerUpperLimits( -180.0, 180.0 );
        xs->m_ZRotate.SetLowerUpperLimits( -180.0, 180.0 );
    }

    if( policy == STACK_LOOP )
    {
        if ( last )
        {
            StackXSec* prevxs = (StackXSec*) m_XSecSurf.FindXSec( indx - 1);
            if( prevxs )
            {
                Matrix4d prevxform;
                prevxform.loadIdentity();

                prevxform.matMult( prevxs->GetTransform()->data() );

                prevxform.affineInverse();
                vec3d offset = prevxform.xform( vec3d( 0.0, 0.0, 0.0 ) );

                xs->m_XDelta.SetLowerUpperLimits( offset.x(), offset.x() );
                xs->m_YDelta.SetLowerUpperLimits( offset.y(), offset.y() );
                xs->m_ZDelta.SetLowerUpperLimits( offset.z(), offset.z() );

                xs->m_XDelta.Set( offset.x() );
                xs->m_YDelta.Set( offset.y() );
                xs->m_ZDelta.Set( offset.z() );

                vec3d angle = prevxform.getAngles();
                xs->m_XRotate.SetLowerUpperLimits( angle.x(), angle.x() );
                xs->m_YRotate.SetLowerUpperLimits( angle.y(), angle.y() );
                xs->m_ZRotate.SetLowerUpperLimits( angle.z(), angle.z() );

                xs->m_XRotate.Set( angle.x() );
                xs->m_YRotate.Set( angle.y() );
                xs->m_ZRotate.Set( angle.z() );
            }
        }
    }
}