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

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

}

// Main Update Routine
void SSEllipse::Update()
{
    UpdateLVecSize();

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

    int num_pnts = m_Tess();

    vec3d center;

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

    double a = m_ULength() / 2;
    double b = m_WLength() / 2;
    for ( int i = 0 ; i < num_pnts ; i++ )
    {
        double p0 = 2 * PI * ( double )i / num_pnts;
        double p1 = 2 * PI * ( double )( i + 1 ) / num_pnts;
        vec3d pnt = vec3d();
        pnt.set_xyz( a * cos( p0 ) + m_CenterU(), b * sin( p0 ) + m_CenterW(), 0 );
        pnt = transMat2.xform( rotMat.xform( transMat1.xform( pnt ) ) );
        m_LVec[i].SetSP0( pnt );
        pnt.set_xyz( a * cos( p1 ) + m_CenterU(), b * sin( p1 ) + m_CenterW(), 0 );
        pnt = transMat2.xform( rotMat.xform( transMat1.xform( pnt ) ) );
        m_LVec[i].SetSP1( pnt );
        m_LVec[i].Update( geom );
    }

    SubSurface::Update();

}

void PtCloudGeom::GetSelectedPoints( vector < vec3d > &selpts )
{
    Matrix4d transMat = GetTotalTransMat();
    for ( int i = 0 ; i < ( int )m_Pts.size() ; i++ )
    {
        if ( m_Selected[i] )
        {
            selpts.push_back(transMat.xform( m_Pts[i] ) );
        }
    }
}

void PtCloudGeom::UpdateDrawObj()
{
    m_PickDrawObj.m_Type = DrawObj::VSP_PICK_VERTEX;
    m_PickDrawObj.m_GeomID = PICKVERTEXHEADER + m_ID;
    m_PickDrawObj.m_PickSourceID = m_ID;
    m_PickDrawObj.m_FeedbackGroup = getFeedbackGroupName();
    m_PickDrawObj.m_PointSize = 4.0;

    m_HighlightDrawObj.m_PntVec = m_BBox.GetBBoxDrawLines();

    Matrix4d transMat = GetTotalTransMat();
    m_XformPts.resize( m_Pts.size() );
    for ( int i = 0 ; i < ( int )m_Pts.size() ; i++ )
    {
        m_XformPts[i] = transMat.xform( m_Pts[i] );
    }
}

void PtCloudGeom::UpdateBBox()
{
    int i, j;
    m_BBox.Reset();
    Matrix4d transMat = GetTotalTransMat();

    if ( m_Pts.size() > 0 )
    {
        for ( i = 0 ; i < ( int )m_Pts.size() ; i++ )
        {
            m_BBox.Update( transMat.xform( m_Pts[i] ) );
        }
    }
    else
    {
        m_BBox.Update( vec3d( 0.0, 0.0, 0.0 ));
    }
}

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