よく使うベクトル・行列計算関数_3DCG

よく使うベクトル・行列計算関数

ベクトルの加減算や内積・外積・行列計算などはレンダラを作る場合には頻繁に使うため、扱いやすいように１ファイル（ソース）にまとめてしまうのが吉です。

たとえば、(x, y, z)の３つのfloat型を持つものは以下のように定義してしまいましょう。

(x, y, z)の要素を持つベクトル

typedef struct VECTOR3 {
public:

    float x, y, z;
    VECTOR3(){ };
    VECTOR3(float x, float y, float z) {
        (this->x) = x;
        (this->y) = y;
        (this->z) = z;
    };

    VECTOR3 operator + (const VECTOR3 dvec) const {
        VECTOR3 v3;

        v3.x = ((this)->x) + dvec.x;
        v3.y = ((this)->y) + dvec.y;
        v3.z = ((this)->z) + dvec.z;
        return v3;
    };

    VECTOR3 operator - (const VECTOR3 dvec) const {
        VECTOR3 v3;

        v3.x = ((this)->x) - dvec.x;
        v3.y = ((this)->y) - dvec.y;
        v3.z = ((this)->z) - dvec.z;
        return v3;
    };

    VECTOR3 operator * (float fDat) const {
        VECTOR3 v3;

        v3.x = ((this)->x) * fDat;
        v3.y = ((this)->y) * fDat;
        v3.z = ((this)->z) * fDat;

        return v3;
    };

    VECTOR3 operator / (float fDat) const {
        VECTOR3 v3;

        if(fDat == 0.0f){
            return *this;
        }

        v3.x = ((this)->x) / fDat;
        v3.y = ((this)->y) / fDat;
        v3.z = ((this)->z) / fDat;
        return v3;
    };

    VECTOR3& operator += (const VECTOR3 dvec) {
        ((this)->x) += dvec.x;
        ((this)->y) += dvec.y;
        ((this)->z) += dvec.z;
        return *this;
    };

    VECTOR3& operator -= (const VECTOR3 dvec) {
        ((this)->x) -= dvec.x;
        ((this)->y) -= dvec.y;
        ((this)->z) -= dvec.z;
        return *this;
    };

    VECTOR3& operator *= (float fDat) {
        ((this)->x) *= fDat;
        ((this)->y) *= fDat;
        ((this)->z) *= fDat;
        return *this;
    };

    VECTOR3& operator /= (float fDat) {

        if(fDat == 0.0f){
            return *this;
        }

        ((this)->x) /= fDat;
        ((this)->y) /= fDat;
        ((this)->z) /= fDat;
        return *this;
    };

} VECTOR3;

C++ならではですが、これだと以下のようなものが

VECTOR3 v1, v2, v3;

v1.x = 1.0f; v1.y = 2.0f; v1.z = 3.0f;
v2.x = 1.0f; v2.y = 2.0f; v2.z = 3.0f;
v3.x = v1.x + v2.x;
v3.y = v1.y + v2.y;
v3.z = v1.z + v2.z;

以下のようにシンプルに記述できます。

VECTOR3 v1(1.0f, 2.0f, 3.0f);
VECTOR3 v2(1.0f, 2.0f, 3.0f);
VECTOR3 v3;

v3 = v1 + v2;

この(x, y, z)の要素を持つ構造体を使ってベクトル操作関数を作っていきます。

(x, y, z, w)の要素を持つベクトル

ベクトルと行列の乗算などで結果として使います。

typedef struct {
    float x, y, z, w;
} VECTOR4;

ベクトルの長さを求める

float Vec3Length(VECTOR3 *pV)
{
    double x, y, z;
    double len;

    if(!pV) return 0.0f;

    x = (double)(pV->x);
    y = (double)(pV->y);
    z = (double)(pV->z);
    len = sqrt(x * x + y * y + z * z);

    return (float)(len);
}

精度を保つためにdouble型にキャストして利用しています。

注意点としてAthlonなどのCPUの場合、非常に細かい演算にて誤差が発生しとんでもない結果になってしまうことがあります（FPUの処理が甘いのかなぁ。これで、アプリ自身が不安定になることもあります）。ですので、以下のように補正。

float Vec3Length(VECTOR3 *pV)
{
    double x,y,z;
    int scaleF;
    double len;

    if(!pV) return 0.0f;

    scaleF=0;

    x = (double)(pV->x); if(x < 0.0) x = -x;
    y = (double)(pV->y); if(y < 0.0) y = -y;
    z = (double)(pV->z); if(z < 0.0) z = -z;
    if(x < (10e-5)) x = 0.0;
    if(y < (10e-5)) y = 0.0;
    if(z < (10e-5)) z = 0.0;
    if(x == 0.0 && y == 0.0 && z == 0.0) return 0.0f;
    if(x > (10e+5) && y > (10e+5) && z > (10e+5)) {
        x *= 0.00390625;        // x /= 256.0;
        y *= 0.00390625;        // y /= 256.0;
        z *= 0.00390625;        // z /= 256.0;
        scaleF = 1;
    }
    len = sqrt(x * x + y * y + z * z);
    if(scaleF) len *= 256.0;

    return (float)(len);
}

小さすぎる値の場合には0に丸める、大きすぎる値の場合には小さくしてから計算後に戻している、というのが分かりますでしょうか。上記処理ですが、補正の際に乗算が発生してしまってます。浮動小数点のフォーマットさえ分かっていれば、指数部をダイレクトにいじってやればもっと最適化できますね。

   float x, y, z;
   double dx, dy, dz;
   int scaleF;
   double dlen;
   float fLen;
   long *pX, *pY, *pZ;
   int ex, ey, ez;

   x = pV->x;
   y = pV->y;
   z = pV->z;

   scaleF = 0;

   pX = (long *)(&x);
   pY = (long *)(&y);
   pZ = (long *)(&z);

   //符号をプラスにする
   *pX = (*pX) & 0x7fffffff;
   *pY = (*pY) & 0x7fffffff;
   *pZ = (*pZ) & 0x7fffffff;

   //指数部を取り出して、小さい値の場合は0にする
   ex = (((*pX) & 0x7f800000) >> 23) - 127;
   if(ex < -16) *pX = 0;
   ey = (((*pY) & 0x7f800000) >> 23) - 127;
   if(ey < -16) *pY = 0;
   ez = (((*pZ) & 0x7f800000) >> 23) - 127;
   if(ez < -16) *pZ = 0;

   if(!(*pX) && !(*pY) && !(*pZ)) return 0.0f;

   if(ex > 16 && ey > 16 && ez > 16) {
       // x /= 256.0;
       *pX = ((ex - 8 + 127) << 23) | ((*pX) & 0x007fffff);
       // y /= 256.0;
       *pY = ((ey - 8 + 127) << 23) | ((*pY) & 0x007fffff);
       // z /= 256.0;
       *pZ = ((ez - 8 + 127) << 23) | ((*pZ) & 0x007fffff);
       scaleF = 1;
   }
   dx = x;
   dy = y;
   dz = z;

   fLen = (float)sqrt(dx * dx + dy * dy + dz * dz);
   if(scaleF) {
       pX = (long *)(&fLen);
       ex = (((*pX) & 0x7f800000) >> 23) - 127;
       //fLen * 256.0
       *pX = ((ex + 8 + 127) << 23) | ((*pX) & 0x007fffff);
   }

   return fLen;

と、こんな感じでfloatをlong型のポインタで操作できるようにして指数部を操作しています。以下、このような記述はしませんが意識はするようにしてみてください。

ベクトルを正規化する

ベクトルの長さが1.0になるように正規化（単位ベクトル化）します。方向ベクトルを正規化する場合によく使います。

int Vec3Normalize(VECTOR3 *pOut, VECTOR3 *pV)
{
    double len;
    double x, y, z;

    x = (double)(pV->x);
    y = (double)(pV->y);
    z = (double)(pV->z);
    len = sqrt(x * x + y * y + z * z);

    if(len < (1e-6)) return 0;

    len = 1.0 / len;
    x *= len;
    y *= len;
    z *= len;

    pOut->x = (float)x;
    pOut->y = (float)y;
    pOut->z = (float)z;

    return 1;
}

ベクトルの内積を求める

float Vec3Dot(VECTOR3 *pV1, VECTOR3 *pV2)
{
    return ((pV1->x)*(pV2->x)+(pV1->y)*(pV2->y)+(pV1->z)*(pV2->z));
}

ベクトルの外積を求める

void Vec3Cross(VECTOR3 *pOut, VECTOR3 *pV1, VECTOR3 *pV2)
{
    VECTOR3 vec;
    double x1, y1, z1, x2, y2, z2;
    
    x1 = (double)(pV1->x);
    y1 = (double)(pV1->y);
    z1 = (double)(pV1->z);
    x2 = (double)(pV2->x);
    y2 = (double)(pV2->y);
    z2 = (double)(pV2->z);
 
    vec.x = (float)(y1 * z2 - z1 * y2);
    vec.y = (float)(z1 * x2 - x1 * z2);
    vec.z = (float)(x1 * y2 - y1 * x2);
    *pOut = vec;
}    

行列の管理構造体

レンダラでは行列として4x4行列をよく使います。それを以下のように定義しています。

typedef struct {
    float m[4][4];
} MATRIX;

行列の並びとしては以下の感じですね。

m[0][0] m[0][1] m[0][2] m[0][3]
m[1][0] m[1][1] m[1][2] m[1][3]
m[2][0] m[2][1] m[2][2] m[2][3]
m[3][0] m[3][1] m[3][2] m[3][3]

単位行列を求める

[1 0 0 0]
[0 1 0 0]
[0 0 1 0]
[0 0 0 1]

に初期化します。

void MatrixIdentity(MATRIX *pOut)
{
    int y;
    float *p1;
 
    for(y = 0; y < 4; y++) {
        p1=pOut->m[y];
        *p1     = 0.0f;
        *(p1+1) = 0.0f;
        *(p1+2) = 0.0f;
        *(p1+3) = 0.0f;
    }
    pOut->m[0][0] = 1.0f;
    pOut->m[1][1] = 1.0f;
    pOut->m[2][2] = 1.0f;
    pOut->m[3][3] = 1.0f;
}

行列同士の乗算

void MatrixMultiply(MATRIX *pOut, MATRIX *pM1, MATRIX *pM2)
{
    int x, y;
    float mx0, mx1, mx2, mx3;
    float *p1, *p2;
    MATRIX mat;
    MATRIX *pRetOut;

    pRetOut = &mat;
    for(y = 0; y < 4; y++) {
        p1  = pM1->m[y];
        mx0 = *p1;
        mx1 = *(p1+1);
        mx2 = *(p1+2);
        mx3 = *(p1+3);

        p2 = pRetOut->m[y];
        for(x = 0; x < 4; x++) {
            *p2 =   mx0 * (pM2->m[0][x]) + mx1 * (pM2->m[1][x]) +
                    mx2 * (pM2->m[2][x]) + mx3 * (pM2->m[3][x]);
            p2++;
        }
    }

    *pOut = mat;
}

(x, y, z)ベクトルと行列の乗算

void Vec3Transform(VECTOR4 *pOut, VECTOR3 *pV, MATRIX *pM)
{
    float vx, vy, vz;
    VECTOR4 vec;
    float *pF1, *pF2, *pF3, *pF4;

    vx = pV->x;
    vy = pV->y;
    vz = pV->z;

    pF1 = pM->m[0];
    pF2 = pM->m[1];
    pF3 = pM->m[2];
    pF4 = pM->m[3];

    vec.x = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.y = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.z = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.w = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + (*pF4);

    *pOut = vec;
}

(x, y, z, w)ベクトルと行列の乗算

void Vec4Transform(VECTOR4 *pOut, VECTOR4 *pV, MATRIX *pM)
{
    float vx, vy, vz, vw;
    VECTOR4 vec;
    float *pF1, *pF2, *pF3, *pF4;

    vx = pV->x;
    vy = pV->y;
    vz = pV->z;
    vw = pV->w;

    pF1 = pM->m[0];
    pF2 = pM->m[1];
    pF3 = pM->m[2];
    pF4 = pM->m[3];
    vec.x = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + vw * (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.y = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + vw * (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.z = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + vw * (*pF4);
    pF1++; pF2++; pF3++; pF4++;

    vec.w = vx * (*pF1) + vy * (*pF2) + vz * (*pF3) + vw * (*pF4);

    *pOut = vec;
}

4x4の逆行列を求める

Aを4x4行列とし、Pを(x, y, z, w)のベクトルとした場合、「P' = A * P」のときの「P = B * P'」を求めます。このBの行列をAの「逆行列」と言います（B = A^(-1)）。

ローカル座標からワールド座標への変換行列がある場合に、逆のワールド座標からローカル座標への変換行列を求める（これが逆行列）、といったことができます。

この処理は複雑なんですが、アルゴリズムについては「ガウスの消去法」を検索してみてください。単純に説明すると、4x4行列の場合は8x4行列を定義しその左半分に4x4行列を、右半分に単位行列を入れます。で、行列の行を足したり引いたり、行を0以外の数でスカラー倍する、行ごと入れ替えることにより、行列の左半分を単位行列に変換します。このときの右半分が求める逆変換行列となります。

ただし、逆行列を求めることができない可能性もありますので、以下関数では求められない場合は戻り値で0を返しています。

int MatrixInverse(MATRIX *pOut, MATRIX *pM)
{
    MATRIX mat;
    int i, j, loop;
    double fDat, fDat2;
    double mat_8x4[4][8];
    int flag;
    float *pF;
    double *pD;

    //8 x 4行列に値を入れる
    for(i = 0; i < 4; i++) {
        pF = pM->m[i];
        for(j = 0; j < 4; j++, pF++) mat_8x4[i][j] = (double)(*pF);
        pD  = mat_8x4[i] + 4;
        for(j = 0; j < 4; j++) {
            if(i == j)   *pD = 1.0;
            else         *pD = 0.0;
            pD++;
        }
    }

    flag = 1;
    for(loop = 0; loop < 4; loop++) {
        fDat = mat_8x4[loop][loop];
        if(fDat != 1.0) {
            if(fDat == 0.0) {
                for(i = loop + 1; i < 4; i++) {
                    fDat = mat_8x4[i][loop];
                    if(fDat != 0.0) break;
                }
                if(i >= 4) {
                    flag = 0;
                    break;
                }
                //行を入れ替える
                for(j = 0; j < 8; j++) {
                    fDat = mat_8x4[i][j];
                    mat_8x4[i][j] = mat_8x4[loop][j];
                    mat_8x4[loop][j] = fDat;
                }
                fDat = mat_8x4[loop][loop];
            }

            for(i = 0; i < 8; i++) mat_8x4[loop][i] /= fDat;
        }
        for(i = 0; i < 4; i++) {
            if(i != loop) {
                fDat = mat_8x4[i][loop];
                if(fDat != 0.0f) {
                    //mat[i][loop]をmat[loop]の行にかけて
                    //(mat[j] - mat[loop] * fDat)を計算
                    for(j = 0; j < 8; j++) {
                        fDat2 = mat_8x4[loop][j] * fDat;
                        mat_8x4[i][j] -= fDat2;
                    }
                }
            }
        }
    }

    if(flag){
        for(i = 0; i < 4; i++) {
            pF = mat.m[i];
            pD = mat_8x4[i] + 4;
            for(j = 0; j < 4; j++) {
                *pF = (float)(*pD);
                pF++;
                pD++;
            }
        }
    } else {
        //単位行列を求める
        S3D_MatrixIdentity(&mat);
    }

    *pOut = mat;

    if(flag) return 1;
    return 0;
}

後、計算途中に精度が落ちる可能性が大いにありますのでdouble型で計算しています。

Shadeプラグインでは、実は「ワールド座標からデバイス（スクリーン）座標」への透視変換行列をinv関数で逆行列をとっても、正常な値が返ってきません。これは、処理をはしょっているためで上のガウスの消去法で逆行列を求めるとその問題も解決します。

そのほか、平行移動や回転・拡大縮小の行列操作関数なども適宜作っておくとよいでしょう。ベクトル・行列計算関数は非常によく使います。ですので、3D関連ではうまいことまとめておいて利用できるようにしておくと後々便利です。