00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <stdlib.h>
00027 #include <memory.h>
00028 #include <assert.h>
00029
00030 #include "utility.h"
00031 #include "vec3.h"
00032 #include "mat3.h"
00033 #include "quaternion.h"
00034
00035 kmMat3* kmMat3Fill(kmMat3* pOut, const kmScalar* pMat)
00036 {
00037 memcpy(pOut->mat, pMat, sizeof(kmScalar) * 9);
00038 return pOut;
00039 }
00040
00042 kmMat3* kmMat3Identity(kmMat3* pOut)
00043 {
00044 memset(pOut->mat, 0, sizeof(float) * 9);
00045 pOut->mat[0] = pOut->mat[4] = pOut->mat[8] = 1.0f;
00046 return pOut;
00047 }
00048
00049 kmScalar kmMat3Determinant(const kmMat3* pIn)
00050 {
00051 kmScalar output;
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 output = pIn->mat[0] * pIn->mat[4] * pIn->mat[8] + pIn->mat[1] * pIn->mat[5] * pIn->mat[6] + pIn->mat[2] * pIn->mat[3] * pIn->mat[7];
00062 output -= pIn->mat[2] * pIn->mat[4] * pIn->mat[6] + pIn->mat[0] * pIn->mat[5] * pIn->mat[7] + pIn->mat[1] * pIn->mat[3] * pIn->mat[8];
00063
00064 return output;
00065 }
00066
00067
00068 kmMat3* kmMat3Adjugate(kmMat3* pOut, const kmMat3* pIn)
00069 {
00070 pOut->mat[0] = pIn->mat[4] * pIn->mat[8] - pIn->mat[5] * pIn->mat[7];
00071 pOut->mat[1] = pIn->mat[2] * pIn->mat[7] - pIn->mat[1] * pIn->mat[8];
00072 pOut->mat[2] = pIn->mat[1] * pIn->mat[5] - pIn->mat[2] * pIn->mat[4];
00073 pOut->mat[3] = pIn->mat[5] * pIn->mat[6] - pIn->mat[3] * pIn->mat[8];
00074 pOut->mat[4] = pIn->mat[0] * pIn->mat[8] - pIn->mat[2] * pIn->mat[6];
00075 pOut->mat[5] = pIn->mat[2] * pIn->mat[3] - pIn->mat[0] * pIn->mat[5];
00076 pOut->mat[6] = pIn->mat[3] * pIn->mat[7] - pIn->mat[4] * pIn->mat[6];
00077 pOut->mat[7] = pIn->mat[1] * pIn->mat[6] - pIn->mat[9] * pIn->mat[7];
00078 pOut->mat[8] = pIn->mat[0] * pIn->mat[4] - pIn->mat[1] * pIn->mat[3];
00079
00080 return pOut;
00081 }
00082
00083 kmMat3* kmMat3Inverse(kmMat3* pOut, const kmScalar pDeterminate, const kmMat3* pM)
00084 {
00085 kmScalar detInv;
00086 kmMat3 adjugate;
00087
00088 if(pDeterminate == 0.0)
00089 {
00090 return NULL;
00091 }
00092
00093 detInv = 1.0 / pDeterminate;
00094
00095 kmMat3Adjugate(&adjugate, pM);
00096 kmMat3ScalarMultiply(pOut, &adjugate, detInv);
00097
00098 return pOut;
00099 }
00100
00102 int kmMat3IsIdentity(const kmMat3* pIn)
00103 {
00104 static const float identity [] = { 1.0f, 0.0f, 0.0f,
00105 0.0f, 1.0f, 0.0f,
00106 0.0f, 0.0f, 1.0f};
00107
00108 return (memcmp(identity, pIn->mat, sizeof(float) * 9) == 0);
00109 }
00110
00112 kmMat3* kmMat3Transpose(kmMat3* pOut, const kmMat3* pIn)
00113 {
00114 int z, x;
00115 for (z = 0; z < 3; ++z) {
00116 for (x = 0; x < 3; ++x) {
00117 pOut->mat[(z * 3) + x] = pIn->mat[(x * 3) + z];
00118 }
00119 }
00120
00121 return pOut;
00122 }
00123
00124
00125 kmMat3* kmMat3Multiply(kmMat3* pOut, const kmMat3* pM1, const kmMat3* pM2)
00126 {
00127 float mat[9];
00128
00129 const float *m1 = pM1->mat, *m2 = pM2->mat;
00130
00131 mat[0] = m1[0] * m2[0] + m1[3] * m2[1] + m1[6] * m2[2];
00132 mat[1] = m1[1] * m2[0] + m1[4] * m2[1] + m1[7] * m2[2];
00133 mat[2] = m1[2] * m2[0] + m1[5] * m2[1] + m1[8] * m2[2];
00134
00135 mat[3] = m1[0] * m2[3] + m1[3] * m2[4] + m1[6] * m2[5];
00136 mat[4] = m1[1] * m2[3] + m1[4] * m2[4] + m1[7] * m2[5];
00137 mat[5] = m1[2] * m2[3] + m1[5] * m2[4] + m1[8] * m2[5];
00138
00139 mat[6] = m1[0] * m2[6] + m1[3] * m2[7] + m1[6] * m2[8];
00140 mat[7] = m1[1] * m2[6] + m1[4] * m2[7] + m1[7] * m2[8];
00141 mat[8] = m1[2] * m2[6] + m1[5] * m2[7] + m1[8] * m2[8];
00142
00143 memcpy(pOut->mat, mat, sizeof(float)*9);
00144
00145 return pOut;
00146 }
00147
00148 kmMat3* kmMat3ScalarMultiply(kmMat3* pOut, const kmMat3* pM, const kmScalar pFactor)
00149 {
00150 float mat[9];
00151 int i;
00152
00153 for(i = 0; i < 9; i++)
00154 {
00155 mat[i] = pM->mat[i] * pFactor;
00156 }
00157
00158 memcpy(pOut->mat, mat, sizeof(float)*9);
00159
00160 return pOut;
00161 }
00162
00164 kmMat3* kmMat3Assign(kmMat3* pOut, const kmMat3* pIn)
00165 {
00166 assert(pOut != pIn);
00167
00168 memcpy(pOut->mat, pIn->mat, sizeof(float)*9);
00169
00170 return pOut;
00171 }
00172
00174 int kmMat3AreEqual(const kmMat3* pMat1, const kmMat3* pMat2)
00175 {
00176 int i;
00177 assert(pMat1 != pMat2);
00178
00179 for (i = 0; i < 9; ++i) {
00180 if (!(pMat1->mat[i] + kmEpsilon > pMat2->mat[i] &&
00181 pMat1->mat[i] - kmEpsilon < pMat2->mat[i])) {
00182 return KM_FALSE;
00183 }
00184 }
00185
00186 return KM_FALSE;
00187 }
00188
00189
00190 kmMat3* kmMat3Rotation(kmMat3* pOut, const float radians)
00191 {
00192
00193
00194
00195
00196
00197
00198 pOut->mat[0] = cosf(radians);
00199 pOut->mat[1] = sinf(radians);
00200 pOut->mat[2] = 0.0f;
00201
00202 pOut->mat[3] = -sinf(radians);
00203 pOut->mat[4] = cosf(radians);
00204 pOut->mat[5] = 0.0f;
00205
00206 pOut->mat[6] = 0.0f;
00207 pOut->mat[7] = 0.0f;
00208 pOut->mat[8] = 1.0f;
00209
00210 return pOut;
00211 }
00212
00214 kmMat3* kmMat3Scaling(kmMat3* pOut, const kmScalar x, const kmScalar y)
00215 {
00216 memset(pOut->mat, 0, sizeof(float) * 9);
00217 pOut->mat[0] = x;
00218 pOut->mat[4] = y;
00219 pOut->mat[8] = 1.0;
00220
00221 return pOut;
00222 }
00223
00224 kmMat3* kmMat3Translation(kmMat3* pOut, const kmScalar x, const kmScalar y)
00225 {
00226 memset(pOut->mat, 0, sizeof(float) * 9);
00227 pOut->mat[6] = x;
00228 pOut->mat[7] = y;
00229 pOut->mat[8] = 1.0;
00230
00231 return pOut;
00232 }
00233
00234