initial.cpp.svn-base

絲路server源碼 Silk Road server source

#include "G3D/platform.h"
#include "G3D/format.h"
#include <memory.h>
#include <assert.h>
#include "G3D/Initil.h"
#include "G3D/g3dmath.h"
#include "G3D/Quat.h"

bool Initil::toEulerAnglesYXZ (float& rfYAngle, float& rfXAngle,
                                float& rfZAngle) const {
    // rot =  cy*cz+sx*sy*sz  cz*sx*sy-cy*sz  cx*sy
    //        cx*sz           cx*cz          -sx
    //       -cz*sy+cy*sx*sz  cy*cz*sx+sy*sz  cx*cy

    if ( elt[1][2] < 1.0 ) {
        if ( elt[1][2] > -1.0 ) {
            rfYAngle = G3D::aTan2(elt[0][2], elt[2][2]);
            rfXAngle = (float) asin( -elt[1][2]);
            rfZAngle = G3D::aTan2(elt[1][0], elt[1][1]);
            return true;
        } else {
            // WARNING.  Not unique.  YA - ZA = atan2(r01,r00)
            rfYAngle = G3D::aTan2(elt[0][1], elt[0][0]);
            rfXAngle = (float)G3D_HALF_PI;
            rfZAngle = 0.0;
            return false;
        }
    } else {
        // WARNING.  Not unique.  YA + ZA = atan2(-r01,r00)
        rfYAngle = G3D::aTan2( -elt[0][1], elt[0][0]);
        rfXAngle = -(float)G3D_HALF_PI;
        rfZAngle = 0.0f;
        return false;
    }
}

//----------------------------------------------------------------------------
bool Initil::toEulerAnglesYZX (float& rfYAngle, float& rfZAngle,
                                float& rfXAngle) const {
    // rot =  cy*cz           sx*sy-cx*cy*sz  cx*sy+cy*sx*sz
    //        sz              cx*cz          -cz*sx
    //       -cz*sy           cy*sx+cx*sy*sz  cx*cy-sx*sy*sz

    if ( elt[1][0] < 1.0 ) {
        if ( elt[1][0] > -1.0 ) {
            rfYAngle = G3D::aTan2( -elt[2][0], elt[0][0]);
            rfZAngle = (float) asin(elt[1][0]);
            rfXAngle = G3D::aTan2( -elt[1][2], elt[1][1]);
            return true;
        } else {
            // WARNING.  Not unique.  YA - XA = -atan2(r21,r22);
            rfYAngle = -G3D::aTan2(elt[2][1], elt[2][2]);
            rfZAngle = -(float)G3D_HALF_PI;
            rfXAngle = 0.0;
            return false;
        }
    } else {
        // WARNING.  Not unique.  YA + XA = atan2(r21,r22)
        rfYAngle = G3D::aTan2(elt[2][1], elt[2][2]);
        rfZAngle = (float)G3D_HALF_PI;
        rfXAngle = 0.0f;
        return false;
    }
}

//----------------------------------------------------------------------------
bool Initil::toEulerAnglesZXY (float& rfZAngle, float& rfXAngle,
                                float& rfYAngle) const {
    // rot =  cy*cz-sx*sy*sz -cx*sz           cz*sy+cy*sx*sz
    //        cz*sx*sy+cy*sz  cx*cz          -cy*cz*sx+sy*sz
    //       -cx*sy           sx              cx*cy

    if ( elt[2][1] < 1.0 ) {
        if ( elt[2][1] > -1.0 ) {
            rfZAngle = G3D::aTan2( -elt[0][1], elt[1][1]);
            rfXAngle = (float) asin(elt[2][1]);
            rfYAngle = G3D::aTan2( -elt[2][0], elt[2][2]);
            return true;
        } else {
            // WARNING.  Not unique.  ZA - YA = -atan(r02,r00)
            rfZAngle = -G3D::aTan2(elt[0][2], elt[0][0]);
            rfXAngle = -(float)G3D_HALF_PI;
            rfYAngle = 0.0f;
            return false;
        }
    } else {
        // WARNING.  Not unique.  ZA + YA = atan2(r02,r00)
        rfZAngle = G3D::aTan2(elt[0][2], elt[0][0]);
        rfXAngle = (float)G3D_HALF_PI;
        rfYAngle = 0.0f;
        return false;
    }
}

//----------------------------------------------------------------------------
bool Initil::toEulerAnglesZYX (float& rfZAngle, float& rfYAngle,
                                float& rfXAngle) const {
    // rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
    //        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
    //       -sy              cy*sx           cx*cy

    if ( elt[2][0] < 1.0 ) {
        if ( elt[2][0] > -1.0 ) {
            rfZAngle = atan2f(elt[1][0], elt[0][0]);
            rfYAngle = asinf(-(double)elt[2][1]);
            rfXAngle = atan2f(elt[2][1], elt[2][2]);
            return true;
        } else {
            // WARNING.  Not unique.  ZA - XA = -atan2(r01,r02)
            rfZAngle = -G3D::aTan2(elt[0][1], elt[0][2]);
            rfYAngle = (float)G3D_HALF_PI;
            rfXAngle = 0.0f;
            return false;
        }
    } else {
        // WARNING.  Not unique.  ZA + XA = atan2(-r01,-r02)
        rfZAngle = G3D::aTan2( -elt[0][1], -elt[0][2]);
        rfYAngle = -(float)G3D_HALF_PI;
        rfXAngle = 0.0f;
        return false;
    }
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesXYZ (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cosf(fYAngle);
    fSin = sinf(fYAngle);
    Initil kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cosf(fPAngle);
    fSin = sinf(fPAngle);
    Initil kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cosf(fRAngle);
    fSin = sinf(fRAngle);
    Initil kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    return kXMat * (kYMat * kZMat);
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesXZY (float fYAngle, float fPAngle,
                                  float fRAngle) {

    float fCos, fSin;

    fCos = cosf(fYAngle);
    fSin = sinf(fYAngle);
    Initil kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cosf(fPAngle);
    fSin = sinf(fPAngle);
    Initil kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cosf(fRAngle);
    fSin = sinf(fRAngle);
    Initil kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    return kXMat * (kZMat * kYMat);
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesYXZ(
    float fYAngle, 
    float fPAngle,
    float fRAngle) {
    
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Initil kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Initil kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Initil kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    return kYMat * (kXMat * kZMat);
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesYZX(
    float fYAngle, 
    float fPAngle,
    float fRAngle) {

    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Initil kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Initil kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Initil kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);

    return kYMat * (kZMat * kXMat);
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesZXY (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Initil kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Initil kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Initil kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    return kZMat * (kXMat * kYMat);
}

//----------------------------------------------------------------------------
Initil Initil::fromEulerAnglesZYX (float fYAngle, float fPAngle,
                                  float fRAngle) {
    float fCos, fSin;

    fCos = cos(fYAngle);
    fSin = sin(fYAngle);
    Initil kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);

    fCos = cos(fPAngle);
    fSin = sin(fPAngle);
    Initil kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);

    fCos = cos(fRAngle);
    fSin = sin(fRAngle);
    Initil kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);

    return kZMat * (kYMat * kXMat);
}

//----------------------------------------------------------------------------
void Initil::tridiagonal (float afDiag[3], float afSubDiag[3]) {
    // Householder reduction T = Q^t M Q
    //   Input:
    //     mat, symmetric 3x3 matrix M