00001
00032 #ifndef LL_LLV4MATRIX3_H
00033 #define LL_LLV4MATRIX3_H
00034
00035 #include "llv4math.h"
00036 #include "llv4vector3.h"
00037 #include "m3math.h"
00038
00039
00040
00041
00042
00043
00044
00045 LL_LLV4MATH_ALIGN_PREFIX
00046
00047 class LLV4Matrix3
00048 {
00049 public:
00050 union {
00051 F32 mMatrix[LLV4_NUM_AXIS][LLV4_NUM_AXIS];
00052 V4F32 mV[LLV4_NUM_AXIS];
00053 };
00054
00055 void lerp(const LLV4Matrix3 &a, const LLV4Matrix3 &b, const F32 &w);
00056 void multiply(const LLVector3 &a, LLVector3& out) const;
00057 void multiply(const LLVector4 &a, LLV4Vector3& out) const;
00058 void multiply(const LLVector3 &a, LLV4Vector3& out) const;
00059
00060 const LLV4Matrix3& transpose();
00061 const LLV4Matrix3& operator=(const LLMatrix3& a);
00062
00063 operator LLMatrix3() const { return (reinterpret_cast<const LLMatrix4*>(const_cast<const F32*>(&mMatrix[0][0])))->getMat3(); }
00064
00065 friend LLVector3 operator*(const LLVector3& a, const LLV4Matrix3& b);
00066 }
00067
00068 LL_LLV4MATH_ALIGN_POSTFIX;
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 #if LL_VECTORIZE
00079
00080 inline void LLV4Matrix3::lerp(const LLV4Matrix3 &a, const LLV4Matrix3 &b, const F32 &w)
00081 {
00082 __m128 vw = _mm_set1_ps(w);
00083 mV[VX] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VX], a.mV[VX]), vw), a.mV[VX]);
00084 mV[VY] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VY], a.mV[VY]), vw), a.mV[VY]);
00085 mV[VZ] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VZ], a.mV[VZ]), vw), a.mV[VZ]);
00086 }
00087
00088 inline void LLV4Matrix3::multiply(const LLVector3 &a, LLVector3& o) const
00089 {
00090 LLV4Vector3 j;
00091 j.v = _mm_mul_ps(_mm_set1_ps(a.mV[VX]), mV[VX]);
00092 j.v = _mm_add_ps(j.v , _mm_mul_ps(_mm_set1_ps(a.mV[VY]), mV[VY]));
00093 j.v = _mm_add_ps(j.v , _mm_mul_ps(_mm_set1_ps(a.mV[VZ]), mV[VZ]));
00094 o.setVec(j.mV);
00095 }
00096
00097 inline void LLV4Matrix3::multiply(const LLVector4 &a, LLV4Vector3& o) const
00098 {
00099 o.v = _mm_mul_ps(_mm_set1_ps(a.mV[VX]), mV[VX]);
00100 o.v = _mm_add_ps(o.v , _mm_mul_ps(_mm_set1_ps(a.mV[VY]), mV[VY]));
00101 o.v = _mm_add_ps(o.v , _mm_mul_ps(_mm_set1_ps(a.mV[VZ]), mV[VZ]));
00102 }
00103
00104 inline void LLV4Matrix3::multiply(const LLVector3 &a, LLV4Vector3& o) const
00105 {
00106 o.v = _mm_mul_ps(_mm_set1_ps(a.mV[VX]), mV[VX]);
00107 o.v = _mm_add_ps(o.v , _mm_mul_ps(_mm_set1_ps(a.mV[VY]), mV[VY]));
00108 o.v = _mm_add_ps(o.v , _mm_mul_ps(_mm_set1_ps(a.mV[VZ]), mV[VZ]));
00109 }
00110
00111
00112
00113
00114
00115
00116
00117 #else
00118
00119 inline void LLV4Matrix3::lerp(const LLV4Matrix3 &a, const LLV4Matrix3 &b, const F32 &w)
00120 {
00121 mMatrix[VX][VX] = llv4lerp(a.mMatrix[VX][VX], b.mMatrix[VX][VX], w);
00122 mMatrix[VX][VY] = llv4lerp(a.mMatrix[VX][VY], b.mMatrix[VX][VY], w);
00123 mMatrix[VX][VZ] = llv4lerp(a.mMatrix[VX][VZ], b.mMatrix[VX][VZ], w);
00124
00125 mMatrix[VY][VX] = llv4lerp(a.mMatrix[VY][VX], b.mMatrix[VY][VX], w);
00126 mMatrix[VY][VY] = llv4lerp(a.mMatrix[VY][VY], b.mMatrix[VY][VY], w);
00127 mMatrix[VY][VZ] = llv4lerp(a.mMatrix[VY][VZ], b.mMatrix[VY][VZ], w);
00128
00129 mMatrix[VZ][VX] = llv4lerp(a.mMatrix[VZ][VX], b.mMatrix[VZ][VX], w);
00130 mMatrix[VZ][VY] = llv4lerp(a.mMatrix[VZ][VY], b.mMatrix[VZ][VY], w);
00131 mMatrix[VZ][VZ] = llv4lerp(a.mMatrix[VZ][VZ], b.mMatrix[VZ][VZ], w);
00132 }
00133
00134 inline void LLV4Matrix3::multiply(const LLVector3 &a, LLVector3& o) const
00135 {
00136 o.setVec( a.mV[VX] * mMatrix[VX][VX] +
00137 a.mV[VY] * mMatrix[VY][VX] +
00138 a.mV[VZ] * mMatrix[VZ][VX],
00139
00140 a.mV[VX] * mMatrix[VX][VY] +
00141 a.mV[VY] * mMatrix[VY][VY] +
00142 a.mV[VZ] * mMatrix[VZ][VY],
00143
00144 a.mV[VX] * mMatrix[VX][VZ] +
00145 a.mV[VY] * mMatrix[VY][VZ] +
00146 a.mV[VZ] * mMatrix[VZ][VZ]);
00147 }
00148
00149 inline void LLV4Matrix3::multiply(const LLVector4 &a, LLV4Vector3& o) const
00150 {
00151 o.setVec( a.mV[VX] * mMatrix[VX][VX] +
00152 a.mV[VY] * mMatrix[VY][VX] +
00153 a.mV[VZ] * mMatrix[VZ][VX],
00154
00155 a.mV[VX] * mMatrix[VX][VY] +
00156 a.mV[VY] * mMatrix[VY][VY] +
00157 a.mV[VZ] * mMatrix[VZ][VY],
00158
00159 a.mV[VX] * mMatrix[VX][VZ] +
00160 a.mV[VY] * mMatrix[VY][VZ] +
00161 a.mV[VZ] * mMatrix[VZ][VZ]);
00162 }
00163
00164 inline void LLV4Matrix3::multiply(const LLVector3 &a, LLV4Vector3& o) const
00165 {
00166 o.setVec( a.mV[VX] * mMatrix[VX][VX] +
00167 a.mV[VY] * mMatrix[VY][VX] +
00168 a.mV[VZ] * mMatrix[VZ][VX],
00169
00170 a.mV[VX] * mMatrix[VX][VY] +
00171 a.mV[VY] * mMatrix[VY][VY] +
00172 a.mV[VZ] * mMatrix[VZ][VY],
00173
00174 a.mV[VX] * mMatrix[VX][VZ] +
00175 a.mV[VY] * mMatrix[VY][VZ] +
00176 a.mV[VZ] * mMatrix[VZ][VZ]);
00177 }
00178
00179
00180
00181
00182
00183
00184
00185 #endif
00186
00187 inline const LLV4Matrix3& LLV4Matrix3::transpose()
00188 {
00189 #if LL_VECTORIZE && defined(_MM_TRANSPOSE4_PS)
00190 _MM_TRANSPOSE4_PS(mV[VX], mV[VY], mV[VZ], mV[VW]);
00191 return *this;
00192 #else
00193 F32 temp;
00194 temp = mMatrix[VX][VY]; mMatrix[VX][VY] = mMatrix[VY][VX]; mMatrix[VY][VX] = temp;
00195 temp = mMatrix[VX][VZ]; mMatrix[VX][VZ] = mMatrix[VZ][VX]; mMatrix[VZ][VX] = temp;
00196 temp = mMatrix[VY][VZ]; mMatrix[VY][VZ] = mMatrix[VZ][VY]; mMatrix[VZ][VY] = temp;
00197 #endif
00198 return *this;
00199 }
00200
00201 inline const LLV4Matrix3& LLV4Matrix3::operator=(const LLMatrix3& a)
00202 {
00203 memcpy(mMatrix[VX], a.mMatrix[VX], sizeof(F32) * 3 );
00204 memcpy(mMatrix[VY], a.mMatrix[VY], sizeof(F32) * 3 );
00205 memcpy(mMatrix[VZ], a.mMatrix[VZ], sizeof(F32) * 3 );
00206 return *this;
00207 }
00208
00209 inline LLVector3 operator*(const LLVector3& a, const LLV4Matrix3& b)
00210 {
00211 return LLVector3(
00212 a.mV[VX] * b.mMatrix[VX][VX] +
00213 a.mV[VY] * b.mMatrix[VY][VX] +
00214 a.mV[VZ] * b.mMatrix[VZ][VX],
00215
00216 a.mV[VX] * b.mMatrix[VX][VY] +
00217 a.mV[VY] * b.mMatrix[VY][VY] +
00218 a.mV[VZ] * b.mMatrix[VZ][VY],
00219
00220 a.mV[VX] * b.mMatrix[VX][VZ] +
00221 a.mV[VY] * b.mMatrix[VY][VZ] +
00222 a.mV[VZ] * b.mMatrix[VZ][VZ] );
00223 }
00224
00225 #endif
