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

void PropGeom::UpdateDrawObj()
{
    GeomXSec::UpdateDrawObj();

    double axlen = 1.0;

    Vehicle *veh = VehicleMgr.GetVehicle();
    if ( veh )
    {
        axlen = veh->m_AxisLength();
    }

    double rev = 1.0;
    if ( m_ReverseFlag() )
    {
        rev = -1.0;
    }

    double data[16];
    m_ModelMatrix.getMat( data );

    vec3d cen( 0, 0, 0 );
    vec3d rotdir( -1, 0, 0 );
    vec3d thrustdir( -1, 0, 0 );
    rotdir = rotdir * rev;

    cen = m_ModelMatrix.xform( cen );
    rotdir = m_ModelMatrix.xform( rotdir ) - cen;
    thrustdir = m_ModelMatrix.xform( thrustdir ) - cen;

    Matrix4d mat;
    mat.loadIdentity();
    mat.rotateX( -rev * m_Rotate() );
    mat.postMult( data );

    vec3d pmid = mat.xform( m_FoldAxOrigin );
    vec3d ptstart = mat.xform( m_FoldAxOrigin + m_FoldAxDirection * axlen / 2.0 );
    vec3d ptend = mat.xform( m_FoldAxOrigin - m_FoldAxDirection * axlen / 2.0 );


    vec3d dir = ptend - ptstart;
    dir.normalize();

    m_ArrowLinesDO.m_PntVec.clear();
    m_ArrowHeadDO.m_PntVec.clear();

    m_ArrowLinesDO.m_PntVec.push_back( ptstart );
    m_ArrowLinesDO.m_PntVec.push_back( ptend );
    m_ArrowLinesDO.m_PntVec.push_back( cen );
    m_ArrowLinesDO.m_PntVec.push_back( cen + thrustdir * axlen );

    MakeArrowhead( cen + thrustdir * axlen, thrustdir, 0.25 * axlen, m_ArrowHeadDO.m_PntVec );
    MakeCircleArrow( pmid, dir, 0.5 * axlen, m_ArrowLinesDO, m_ArrowHeadDO );
    MakeCircleArrow( cen, rotdir, 0.5 * axlen, m_ArrowLinesDO, m_ArrowHeadDO );

}

void SSRectangle::Update()
{
    Geom* geom = VehicleMgr.GetVehicle()->FindGeom( m_CompID );
    if ( !geom )
    {
        return;
    }

    vec3d center;
    vector< vec3d > pntVec;

    center = vec3d( m_CenterU(), m_CenterW(), 0 );

    // Rotation Matrix
    Matrix4d transMat1, transMat2;
    Matrix4d rotMat;
    rotMat.loadIdentity();
    rotMat.rotateZ( m_Theta() );
    transMat1.loadIdentity();
    transMat1.translatef( center.x() * -1, center.y() * -1, 0 );
    transMat2.loadIdentity();
    transMat2.translatef( center.x(), center.y(), 0 );

    // Make points in counter clockwise fashion;
    pntVec.resize( 5 );
    pntVec[0] = center + vec3d( m_ULength(), m_WLength(), 0 ) * -0.5;
    pntVec[1] = center + vec3d( m_ULength(), -1.0 * m_WLength(), 0 ) * 0.5;
    pntVec[2] = center + vec3d( m_ULength(), m_WLength(), 0 ) * 0.5;
    pntVec[3] = center + vec3d( -1.0 * m_ULength(), m_WLength(), 0 ) * 0.5;
    pntVec[4] = pntVec[0];

    // Apply transformations
    for ( int i = 0; i < 5 ; i++ )
    {
        pntVec[i] = transMat2.xform( rotMat.xform( transMat1.xform( pntVec[i] ) ) );
    }

    int pind = 0;

    for ( int i = 0 ; i < 4; i++ )
    {
        m_LVec[i].SetSP0( pntVec[pind] );
        pind++;
        m_LVec[i].SetSP1( pntVec[pind] );
        m_LVec[i].Update( geom );
    }

    SubSurface::Update();
}

void PropPositioner::Update()
{
    m_NeedsUpdate = false;

    m_TransformedCurve = m_Curve;

    Matrix4d mat;
    mat.scale( m_Chord );

    if ( m_Reverse < 0 )
    {
        mat.translatef( 1.0, 0.0, 0.0 );
        mat.flipx();
    }

    m_TransformedCurve.Transform( mat );

    if ( !m_ParentProp )
    {
        return;
    }

    // Basic transformation orients curve before other transformations.
    m_ParentProp->GetBasicTransformation( m_Chord, mat );
    m_TransformedCurve.Transform( mat );

    mat.loadIdentity();

    // Propeller rotation first because order is reversed.
    mat.rotateX( -m_Reverse * m_PropRot );

    mat.translatef( m_FoldOrigin.x(), m_FoldOrigin.y(), m_FoldOrigin.z() );
    mat.rotate( m_FoldAngle * PI / 180.0, m_FoldDirection );
    mat.translatef( -m_FoldOrigin.x(), -m_FoldOrigin.y(), -m_FoldOrigin.z() );

    mat.rotateY( m_Reverse * m_Feather );

    mat.translatef( 0, m_Radius, 0 );

    mat.rotateY( m_Reverse * m_Twist );

    mat.rotateX( m_XRotate ); // About rake direction

    mat.translatef( m_Rake, 0, m_Reverse * m_Skew );

    mat.rotateZ( m_ZRotate ); // About chord

    m_TransformedCurve.Transform( mat );
}

//.........这里部分代码省略.........
    m_AF.Deactivate();

    if ( m_UseBeta34Flag() == 1 )
    {
        if ( rfirst <= 0.75 )
        {
            double theta34 = m_TwistCurve.Comp( 0.75 );
            m_Feather = m_Beta34() - theta34;
        }
        else
        {
            m_Feather = m_Beta34();
        }
        m_Beta34.Activate();
        m_Feather.Deactivate();
    }
    else
    {
        if ( rfirst <= 0.75 )
        {
            double theta34 = m_TwistCurve.Comp( 0.75 );
            m_Beta34 = m_Feather() + theta34;
        }
        else
        {
            m_Beta34 = m_Feather();
        }
        m_Beta34.Deactivate();
        m_Feather.Activate();
    }

    // Set up fold axis & store for visualization.
    Matrix4d fold, foldrot;
    fold.loadIdentity();
    fold.rotateY( m_AzimuthFoldDir() );
    fold.rotateX( m_ElevationFoldDir() );
    m_FoldAxDirection = fold.xform( vec3d( 0, 0, 1 ) );
    m_FoldAxOrigin = vec3d( m_AxialFoldAxis() * radius, m_RadFoldAxis() * radius, m_OffsetFoldAxis() * radius );

    //==== Cross Section Curves & joint info ====//
    vector< VspCurve > crv_vec;
    crv_vec.resize( nxsec );

    //==== Update XSec Location/Rotation ====//
    for ( int i = 0 ; i < nxsec ; i++ )
    {
        PropXSec* xs = ( PropXSec* ) m_XSecSurf.FindXSec( i );

        if ( xs )
        {
            XSecCurve* xsc = xs->GetXSecCurve();

            double r = xs->m_RadiusFrac();
            double w = m_ChordCurve.Comp( r ) * radius;

            if ( xsc )
            {
                //==== Find Width Parm ====//
                string width_id = xsc->GetWidthParmID();
                Parm* width_parm = ParmMgr.FindParm( width_id );

                piecewise_curve_type pwc;

                if ( width_parm )
                {
                    width_parm->Deactivate();

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() );
            }
        }
    }
}

void StackGeom::AddDefaultSources( double base_len )
{
    switch ( m_OrderPolicy() )
    {
    case STACK_FREE:
    {

        StackXSec* lastxs = (StackXSec*) m_XSecSurf.FindXSec( m_XSecSurf.NumXSec() - 1);
        if( lastxs )
        {
            Matrix4d prevxform;
            prevxform.loadIdentity();

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

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

            double len = offset.mag();

            AddDefaultSourcesXSec( base_len, len, 0 );
            AddDefaultSourcesXSec( base_len, len, m_XSecSurf.NumXSec() - 1 );
        }


        break;
    }
    case STACK_LOOP:
    {
        int iback = -1;
        double dfront = -1.0;

        for ( int i = 0 ; i < m_XSecSurf.NumXSec() ; i++ )
        {
            StackXSec* xs = ( StackXSec* ) m_XSecSurf.FindXSec( i );
            if ( xs )
            {
                Matrix4d prevxform;
                prevxform.loadIdentity();

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

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

                double len = offset.mag();

                if ( len > dfront )
                {
                    dfront = len;
                    iback = i;
                }

            }
        }

        AddDefaultSourcesXSec( base_len, dfront, 0 );
        if ( iback >= 0 )
        {
            AddDefaultSourcesXSec( base_len, dfront, iback );
        }

        break;
    }
    }
}