imathmatrix.h

对gif

		     x[3][1] * a,
		     x[3][2] * a,
		     x[3][3] * a);
}

template <class T>
inline Matrix44<T>
operator * (T a, const Matrix44<T> &v)
{
    return v * a;
}

template <class T>
inline const Matrix44<T> &
Matrix44<T>::operator *= (const Matrix44<T> &v)
{
    Matrix44 tmp (T (0));

    multiply (*this, v, tmp);
    *this = tmp;
    return *this;
}

template <class T>
inline Matrix44<T>
Matrix44<T>::operator * (const Matrix44<T> &v) const
{
    Matrix44 tmp (T (0));

    multiply (*this, v, tmp);
    return tmp;
}

template <class T>
void
Matrix44<T>::multiply (const Matrix44<T> &a,
		       const Matrix44<T> &b,
		       Matrix44<T> &c)
{
    register const T * restrict ap = &a.x[0][0];
    register const T * restrict bp = &b.x[0][0];
    register       T * restrict cp = &c.x[0][0];

    register T a0, a1, a2, a3;

    a0 = ap[0];
    a1 = ap[1];
    a2 = ap[2];
    a3 = ap[3];

    cp[0]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
    cp[1]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
    cp[2]  = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
    cp[3]  = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];

    a0 = ap[4];
    a1 = ap[5];
    a2 = ap[6];
    a3 = ap[7];

    cp[4]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
    cp[5]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
    cp[6]  = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
    cp[7]  = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];

    a0 = ap[8];
    a1 = ap[9];
    a2 = ap[10];
    a3 = ap[11];

    cp[8]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
    cp[9]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
    cp[10] = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
    cp[11] = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];

    a0 = ap[12];
    a1 = ap[13];
    a2 = ap[14];
    a3 = ap[15];

    cp[12] = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
    cp[13] = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
    cp[14] = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
    cp[15] = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];
}

template <class T> template <class S>
void
Matrix44<T>::multVecMatrix(const Vec3<S> &src, Vec3<S> &dst) const
{
    S a, b, c, w;

    a = src[0] * x[0][0] + src[1] * x[1][0] + src[2] * x[2][0] + x[3][0];
    b = src[0] * x[0][1] + src[1] * x[1][1] + src[2] * x[2][1] + x[3][1];
    c = src[0] * x[0][2] + src[1] * x[1][2] + src[2] * x[2][2] + x[3][2];
    w = src[0] * x[0][3] + src[1] * x[1][3] + src[2] * x[2][3] + x[3][3];

    dst.x = a / w;
    dst.y = b / w;
    dst.z = c / w;
}

template <class T> template <class S>
void
Matrix44<T>::multDirMatrix(const Vec3<S> &src, Vec3<S> &dst) const
{
    S a, b, c;

    a = src[0] * x[0][0] + src[1] * x[1][0] + src[2] * x[2][0];
    b = src[0] * x[0][1] + src[1] * x[1][1] + src[2] * x[2][1];
    c = src[0] * x[0][2] + src[1] * x[1][2] + src[2] * x[2][2];

    dst.x = a;
    dst.y = b;
    dst.z = c;
}

template <class T>
const Matrix44<T> &
Matrix44<T>::operator /= (T a)
{
    x[0][0] /= a;
    x[0][1] /= a;
    x[0][2] /= a;
    x[0][3] /= a;
    x[1][0] /= a;
    x[1][1] /= a;
    x[1][2] /= a;
    x[1][3] /= a;
    x[2][0] /= a;
    x[2][1] /= a;
    x[2][2] /= a;
    x[2][3] /= a;
    x[3][0] /= a;
    x[3][1] /= a;
    x[3][2] /= a;
    x[3][3] /= a;

    return *this;
}

template <class T>
Matrix44<T>
Matrix44<T>::operator / (T a) const
{
    return Matrix44 (x[0][0] / a,
		     x[0][1] / a,
		     x[0][2] / a,
		     x[0][3] / a,
		     x[1][0] / a,
		     x[1][1] / a,
		     x[1][2] / a,
		     x[1][3] / a,
		     x[2][0] / a,
		     x[2][1] / a,
		     x[2][2] / a,
		     x[2][3] / a,
		     x[3][0] / a,
		     x[3][1] / a,
		     x[3][2] / a,
		     x[3][3] / a);
}

template <class T>
const Matrix44<T> &
Matrix44<T>::transpose ()
{
    Matrix44 tmp (x[0][0],
		  x[1][0],
		  x[2][0],
		  x[3][0],
		  x[0][1],
		  x[1][1],
		  x[2][1],
		  x[3][1],
		  x[0][2],
		  x[1][2],
		  x[2][2],
		  x[3][2],
		  x[0][3],
		  x[1][3],
		  x[2][3],
		  x[3][3]);
    *this = tmp;
    return *this;
}

template <class T>
Matrix44<T>
Matrix44<T>::transposed () const
{
    return Matrix44 (x[0][0],
		     x[1][0],
		     x[2][0],
		     x[3][0],
		     x[0][1],
		     x[1][1],
		     x[2][1],
		     x[3][1],
		     x[0][2],
		     x[1][2],
		     x[2][2],
		     x[3][2],
		     x[0][3],
		     x[1][3],
		     x[2][3],
		     x[3][3]);
}

template <class T>
const Matrix44<T> &
Matrix44<T>::gjInvert (bool singExc) throw (Iex::MathExc)
{
    *this = gjInverse (singExc);
    return *this;
}

template <class T>
Matrix44<T>
Matrix44<T>::gjInverse (bool singExc) const throw (Iex::MathExc)
{
    int i, j, k;
    Matrix44 s;
    Matrix44 t (*this);

    // Forward elimination

    for (i = 0; i < 3 ; i++)
    {
	int pivot = i;

	T pivotsize = t[i][i];

	if (pivotsize < 0)
	    pivotsize = -pivotsize;

	for (j = i + 1; j < 4; j++)
	{
	    T tmp = t[j][i];

	    if (tmp < 0)
		tmp = -tmp;

	    if (tmp > pivotsize)
	    {
		pivot = j;
		pivotsize = tmp;
	    }
	}

	if (pivotsize == 0)
	{
	    if (singExc)
		throw ::Imath::SingMatrixExc ("Cannot invert singular matrix.");

	    return Matrix44();
	}

	if (pivot != i)
	{
	    for (j = 0; j < 4; j++)
	    {
		T tmp;

		tmp = t[i][j];
		t[i][j] = t[pivot][j];
		t[pivot][j] = tmp;

		tmp = s[i][j];
		s[i][j] = s[pivot][j];
		s[pivot][j] = tmp;
	    }
	}

	for (j = i + 1; j < 4; j++)
	{
	    T f = t[j][i] / t[i][i];

	    for (k = 0; k < 4; k++)
	    {
		t[j][k] -= f * t[i][k];
		s[j][k] -= f * s[i][k];
	    }
	}
    }

    // Backward substitution

    for (i = 3; i >= 0; --i)
    {
	T f;

	if ((f = t[i][i]) == 0)
	{
	    if (singExc)
		throw ::Imath::SingMatrixExc ("Cannot invert singular matrix.");

	    return Matrix44();
	}

	for (j = 0; j < 4; j++)
	{
	    t[i][j] /= f;
	    s[i][j] /= f;
	}

	for (j = 0; j < i; j++)
	{
	    f = t[j][i];

	    for (k = 0; k < 4; k++)
	    {
		t[j][k] -= f * t[i][k];
		s[j][k] -= f * s[i][k];
	    }
	}
    }

    return s;
}

template <class T>
const Matrix44<T> &
Matrix44<T>::invert (bool singExc) throw (Iex::MathExc)
{
    *this = inverse (singExc);
    return *this;
}

template <class T>
Matrix44<T>
Matrix44<T>::inverse (bool singExc) const throw (Iex::MathExc)
{
    if (x[0][3] != 0 || x[1][3] != 0 || x[2][3] != 0 || x[3][3] != 1)
	return gjInverse(singExc);

    Matrix44 s (x[1][1] * x[2][2] - x[2][1] * x[1][2],
		x[2][1] * x[0][2] - x[0][1] * x[2][2],
		x[0][1] * x[1][2] - x[1][1] * x[0][2],
		0,

		x[2][0] * x[1][2] - x[1][0] * x[2][2],
		x[0][0] * x[2][2] - x[2][0] * x[0][2],
		x[1][0] * x[0][2] - x[0][0] * x[1][2],
		0,

		x[1][0] * x[2][1] - x[2][0] * x[1][1],
		x[2][0] * x[0][1] - x[0][0] * x[2][1],
		x[0][0] * x[1][1] - x[1][0] * x[0][1],
		0,

		0,
		0,
		0,
		1);

    T r = x[0][0] * s[0][0] + x[0][1] * s[1][0] + x[0][2] * s[2][0];

    if (Imath::abs (r) >= 1)
    {
	for (int i = 0; i < 3; ++i)
	{
	    for (int j = 0; j < 3; ++j)
	    {
		s[i][j] /= r;
	    }
	}
    }
    else
    {
	T mr = Imath::abs (r) / limits<T>::smallest();

	for (int i = 0; i < 3; ++i)
	{
	    for (int j = 0; j < 3; ++j)
	    {
		if (mr > Imath::abs (s[i][j]))
		{
		    s[i][j] /= r;
		}
		else
		{
		    if (singExc)
			throw SingMatrixExc ("Cannot invert singular matrix.");

		    return Matrix44();
		}
	    }
	}
    }

    s[3][0] = -x[3][0] * s[0][0] - x[3][1] * s[1][0] - x[3][2] * s[2][0];
    s[3][1] = -x[3][0] * s[0][1] - x[3][1] * s[1][1] - x[3][2] * s[2][1];
    s[3][2] = -x[3][0] * s[0][2] - x[3][1] * s[1][2] - x[3][2] * s[2][2];

    return s;
}

template <class T>
template <class S>
const Matrix44<T> &
Matrix44<T>::setEulerAngles (const Vec3<S>& r)
{
    S cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
    
    cos_rz = Math<T>::cos (r[2]);
    cos_ry = Math<T>::cos (r[1]);
    cos_rx = Math<T>::cos (r[0]);
    
    sin_rz = Math<T>::sin (r[2]);
    sin_ry = Math<T>::sin (r[1]);
    sin_rx = Math<T>::sin (r[0]);
    
    x[0][0] =  cos_rz * cos_ry;
    x[0][1] =  sin_rz * cos_ry;
    x[0][2] = -sin_ry;
    x[0][3] =  0;
    
    x[1][0] = -sin_rz * cos_rx + cos_rz * sin_ry * sin_rx;
    x[1][1] =  cos_rz * cos_rx + sin_rz * sin_ry * sin_rx;
    x[1][2] =  cos_ry * sin_rx;
    x[1][3] =  0;
    
    x[2][0] =  sin_rz * sin_rx + cos_rz * sin_ry * cos_rx;
    x[2][1] = -cos_rz * sin_rx + sin_rz * sin_ry * cos_rx;
    x[2][2] =  cos_ry * cos_rx;
    x[2][3] =  0;

    x[3][0] =  0;
    x[3][1] =  0;
    x[3][2] =  0;
    x[3][3] =  1;

    return *this;
}

template <class T>
template <class S>
const Matrix44<T> &
Matrix44<T>::setAxisAngle (const Vec3<S>& axis, S angle)
{
    Vec3<S> unit (axis.normalized());
    S sine   = Math<T>::sin (angle);
    S cosine = Math<T>::cos (angle);

    x[0][0] = unit[0] * unit[0] * (1 - cosine) + cosine;
    x[0][1] = unit[0] * unit[1] * (1 - cosine) + unit[2] * sine;
    x[0][2] = unit[0] * unit[2] * (1 - cosine) - unit[1] * sine;
    x[0][3] = 0;

    x[1][0] = unit[0] * unit[1] * (1 - cosine) - unit[2] * sine;
    x[1][1] = unit[1] * unit[1] * (1 - cosine) + cosine;
    x[1][2] = unit[1] * unit[2] * (1 - cosine) + unit[0] * sine;
    x[1][3] = 0;

    x[2][0] = unit[0] * unit[2] * (1 - cosine) + unit[1] * sine;
    x[2][1] = unit[1] * unit[2] * (1 - cosine) - unit[0] * sine;
    x[2][2] = unit[2] * unit[2] * (1 - cosine) + cosine;
    x[2][3] = 0;

    x[3][0] = 0;
    x[3][1] = 0;
    x[3][2] = 0;
    x[3][3] = 1;

    return *this;
}

template <class T>
template <class S>
const Matrix44<T> &
Matrix44<T>::rotate (const Vec3<S> &r)
{
    S cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
    S m00, m01, m02;
    S m10, m11, m12;
    S m20, m21, m22;

    cos_rz = Math<S>::cos (r[2]);
    cos_ry = Math<S>::cos (r[1]);
    cos_rx = Math<S>::cos (r[0]);
    
    sin_rz = Math<S>::sin (r[2]);
    sin_ry = Math<S>::sin (r[1]);
    sin_rx = Math<S>::sin (r[0]);

    m00 =  cos_rz *  cos_ry;
    m01 =  sin_rz *  cos_ry;
    m02 = -sin_ry;
    m10 = -sin_rz *  cos_rx + cos_rz * sin_ry * sin_rx;
    m11 =  cos_rz *  cos_rx + sin_rz * sin_ry * sin_rx;
    m12 =  cos_ry *  sin_rx;
    m20 = -sin_rz * -sin_rx + cos_rz * sin_ry * cos_rx;
    m21 =  cos_rz * -sin_rx + sin_rz * sin_ry * cos_rx;
    m22 =  cos_ry *  cos_rx;

    Matrix44<T> P (*this);

    x[0][0] = P[0][0] * m00 + P[1][0] * m01 + P[2][0] * m02;
    x[0][1] = P[0][1] * m00 + P[1][1] * m01 + P[2][1] * m02;
    x[0][2] = P[0][2] * m00 + P[1][2] * m01 + P[2][2] * m02;
    x[0][3] = P[0][3] * m00 + P[1][3] * m01 + P[2][3] * m02;

    x[1][0] = P[0][0] * m10 + P[1][0] * m11 + P[2][0] * m12;
    x[1][1] = P[0][1] * m10 + P[1][1] * m11 + P[2][1] * m12;
    x[1][2] = P[0][2] * m10 + P[1][2] * m11 + P[2][2] * m12;
    x[1][3] = P[0][3] * m10 + P[1][3] * m11 + P[2][3] * m12;

    x[2][0] = P[0][0] * m20 + P[1][0] * m21 + P[2][0] * m22;
    x[2][1] = P[0][1] * m20 + P[1][1] * m21 + P[2][1] * m22;
    x[2][2] = P[0][2] * m20 + P[1][2] * m21 + P[2][2] * m22;
    x[2][3] = P[0][3] * m20 + P[1][3] * m21 + P[2][3] * m22;

    return *this;
}

template <class T>
const Matrix44<T> &
Matrix44<T>::setScale (T s)
{
    x[0][0] = s;
    x[0][1] = 0;
    x[0][2] = 0;
    x[0][3] = 0;

    x[1][0] = 0;
    x[1][1] = s;
    x[1][2] = 0;
    x[1][3] = 0;

    x[2][0] = 0;
    x[2][1] = 0;
    x[2][2] = s;
    x[2][3] = 0;

    x[3][0] = 0;
    x[3][1] = 0;
    x[3][2] = 0;
    x[3][3] = 1;

    return *this;
}

template <class T>
template <class S>
const Matrix44<T> &
Matrix44<T>::setScale (const Vec3<S> &s)
{
    x[0][0] = s[0];
    x[0][1] = 0;
    x[0][2] = 0;
    x[0][3] = 0;

    x[1][0] = 0;
    x[1][1] = s[1];
    x[1][2] = 0;
    x[1][3] = 0;

    x[2][0] = 0;
    x[2][1] = 0;
    x[2][2] = s[2];
    x[2][3] = 0;

    x[3][0] = 0;
    x[3][1] = 0;
    x[3][2] = 0;
    x[3][3] = 1;

    return *this;
}

template <class T>
template <class S>
const Matrix44<T> &
Matrix44<T>::scale (const Vec3<S> &s)
{
    x[0][0] *= s[0];
    x[0][1] *= s[0];
    x[0][2] *= s[0];
    x[0][3] *= s[0];

    x[1][0] *= s[1];
    x[1][1]