matrix4x4.c

   1
   2 #include "matrix4x4.h"
   3 #include <math.h>
   4
   5 void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in)
   6 {
   7         *out = *in;
   8 }
   9
  10 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
  11 {
  12         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
  13         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
  14         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
  15         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
  16         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
  17         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
  18         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
  19         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
  20         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
  21         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
  22         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
  23         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
  24         out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
  25         out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
  26         out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
  27         out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
  28 }
  29
  30 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
  31 {
  32         out->m[0][0] = in1->m[0][0];
  33         out->m[0][1] = in1->m[1][0];
  34         out->m[0][2] = in1->m[2][0];
  35         out->m[0][3] = in1->m[3][0];
  36         out->m[1][0] = in1->m[0][1];
  37         out->m[1][1] = in1->m[1][1];
  38         out->m[1][2] = in1->m[2][1];
  39         out->m[1][3] = in1->m[3][1];
  40         out->m[2][0] = in1->m[0][2];
  41         out->m[2][1] = in1->m[1][2];
  42         out->m[2][2] = in1->m[2][2];
  43         out->m[2][3] = in1->m[3][2];
  44         out->m[3][0] = in1->m[0][3];
  45         out->m[3][1] = in1->m[1][3];
  46         out->m[3][2] = in1->m[2][3];
  47         out->m[3][3] = in1->m[3][3];
  48 }
  49
  50 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
  51 {
  52         out->m[0][0]=1.0f;
  53         out->m[0][1]=0.0f;
  54         out->m[0][2]=0.0f;
  55         out->m[0][3]=0.0f;
  56         out->m[1][0]=0.0f;
  57         out->m[1][1]=1.0f;
  58         out->m[1][2]=0.0f;
  59         out->m[1][3]=0.0f;
  60         out->m[2][0]=0.0f;
  61         out->m[2][1]=0.0f;
  62         out->m[2][2]=1.0f;
  63         out->m[2][3]=0.0f;
  64         out->m[3][0]=0.0f;
  65         out->m[3][1]=0.0f;
  66         out->m[3][2]=0.0f;
  67         out->m[3][3]=1.0f;
  68 }
  69
  70 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
  71 {
  72         out->m[0][0]=1.0f;
  73         out->m[0][1]=0.0f;
  74         out->m[0][2]=0.0f;
  75         out->m[0][3]=x;
  76         out->m[1][0]=0.0f;
  77         out->m[1][1]=1.0f;
  78         out->m[1][2]=0.0f;
  79         out->m[1][3]=y;
  80         out->m[2][0]=0.0f;
  81         out->m[2][1]=0.0f;
  82         out->m[2][2]=1.0f;
  83         out->m[2][3]=z;
  84         out->m[3][0]=0.0f;
  85         out->m[3][1]=0.0f;
  86         out->m[3][2]=0.0f;
  87         out->m[3][3]=1.0f;
  88 }
  89
  90 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
  91 {
  92         float len, c, s;
  93
  94         len = x*x+y*y+z*z;
  95         if (len != 0.0f)
  96                 len = 1.0f / sqrt(len);
  97         x *= len;
  98         y *= len;
  99         z *= len;
 100
 101         angle *= M_PI / 180.0;
 102         c = cos(angle);
 103         s = sin(angle);
 104
 105         out->m[0][0]=x * x + c * (1 - x * x);
 106         out->m[0][1]=x * y * (1 - c) + z * s;
 107         out->m[0][2]=z * x * (1 - c) - y * s;
 108         out->m[0][3]=0.0f;
 109         out->m[1][0]=x * y * (1 - c) - z * s;
 110         out->m[1][1]=y * y + c * (1 - y * y);
 111         out->m[1][2]=y * z * (1 - c) + x * s;
 112         out->m[1][3]=0.0f;
 113         out->m[2][0]=z * x * (1 - c) + y * s;
 114         out->m[2][1]=y * z * (1 - c) - x * s;
 115         out->m[2][2]=z * z + c * (1 - z * z);
 116         out->m[2][3]=0.0f;
 117         out->m[3][0]=0.0f;
 118         out->m[3][1]=0.0f;
 119         out->m[3][2]=0.0f;
 120         out->m[3][3]=1.0f;
 121 }
 122
 123 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
 124 {
 125         out->m[0][0]=x;
 126         out->m[0][1]=0.0f;
 127         out->m[0][2]=0.0f;
 128         out->m[0][3]=0.0f;
 129         out->m[1][0]=0.0f;
 130         out->m[1][1]=x;
 131         out->m[1][2]=0.0f;
 132         out->m[1][3]=0.0f;
 133         out->m[2][0]=0.0f;
 134         out->m[2][1]=0.0f;
 135         out->m[2][2]=x;
 136         out->m[2][3]=0.0f;
 137         out->m[3][0]=0.0f;
 138         out->m[3][1]=0.0f;
 139         out->m[3][2]=0.0f;
 140         out->m[3][3]=1.0f;
 141 }
 142
 143 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
 144 {
 145         out->m[0][0]=x;
 146         out->m[0][1]=0.0f;
 147         out->m[0][2]=0.0f;
 148         out->m[0][3]=0.0f;
 149         out->m[1][0]=0.0f;
 150         out->m[1][1]=y;
 151         out->m[1][2]=0.0f;
 152         out->m[1][3]=0.0f;
 153         out->m[2][0]=0.0f;
 154         out->m[2][1]=0.0f;
 155         out->m[2][2]=z;
 156         out->m[2][3]=0.0f;
 157         out->m[3][0]=0.0f;
 158         out->m[3][1]=0.0f;
 159         out->m[3][2]=0.0f;
 160         out->m[3][3]=1.0f;
 161 }
 162
 163 void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
 164 {
 165         vx[0] = in->m[0][0];
 166         vx[1] = in->m[1][0];
 167         vx[2] = in->m[2][0];
 168         vy[0] = in->m[0][1];
 169         vy[1] = in->m[1][1];
 170         vy[2] = in->m[2][1];
 171         vz[0] = in->m[0][2];
 172         vz[1] = in->m[1][2];
 173         vz[2] = in->m[2][2];
 174         t[0] = in->m[0][3];
 175         t[1] = in->m[1][3];
 176         t[2] = in->m[2][3];
 177 }
 178
 179 void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t)
 180 {
 181         vx[0] = in->m[0][0];
 182         vx[1] = in->m[1][0];
 183         vx[2] = in->m[2][0];
 184         vx[3] = in->m[3][0];
 185         vy[0] = in->m[0][1];
 186         vy[1] = in->m[1][1];
 187         vy[2] = in->m[2][1];
 188         vy[3] = in->m[3][1];
 189         vz[0] = in->m[0][2];
 190         vz[1] = in->m[1][2];
 191         vz[2] = in->m[2][2];
 192         vz[3] = in->m[3][2];
 193         t[0] = in->m[0][3];
 194         t[1] = in->m[1][3];
 195         t[2] = in->m[2][3];
 196         t[3] = in->m[3][3];
 197 }
 198
 199 void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
 200 {
 201         out->m[0][0] = vx[0];
 202         out->m[0][1] = vy[0];
 203         out->m[0][2] = vz[0];
 204         out->m[0][3] = t[0];
 205         out->m[1][0] = vx[1];
 206         out->m[1][1] = vy[1];
 207         out->m[1][2] = vz[1];
 208         out->m[1][3] = t[1];
 209         out->m[2][0] = vx[2];
 210         out->m[2][1] = vy[2];
 211         out->m[2][2] = vz[2];
 212         out->m[2][3] = t[2];
 213         out->m[3][0] = 0.0f;
 214         out->m[3][1] = 0.0f;
 215         out->m[3][2] = 0.0f;
 216         out->m[3][3] = 1.0f;
 217 }
 218
 219 void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t)
 220 {
 221         out->m[0][0] = vx[0];
 222         out->m[0][1] = vy[0];
 223         out->m[0][2] = vz[0];
 224         out->m[0][3] = t[0];
 225         out->m[1][0] = vx[1];
 226         out->m[1][1] = vy[1];
 227         out->m[1][2] = vz[1];
 228         out->m[1][3] = t[1];
 229         out->m[2][0] = vx[2];
 230         out->m[2][1] = vy[2];
 231         out->m[2][2] = vz[2];
 232         out->m[2][3] = t[2];
 233         out->m[3][0] = vx[3];
 234         out->m[3][1] = vy[3];
 235         out->m[3][2] = vz[3];
 236         out->m[3][3] = t[3];
 237 }
 238
 239 void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
 240 {
 241         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 242         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 243         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 244 }
 245
 246 void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
 247 {
 248         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
 249         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
 250         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
 251         out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
 252 }
 253
 254 void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
 255 {
 256         vec3_t t;
 257         t[0] = v[0] - in->m[0][3];
 258         t[1] = v[1] - in->m[1][3];
 259         t[2] = v[2] - in->m[2][3];
 260         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 261         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 262         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 263 }
 264
 265 void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
 266 {
 267         vec3_t t;
 268         t[0] = v[0] - in->m[0][3];
 269         t[1] = v[1] - in->m[1][3];
 270         t[2] = v[2] - in->m[2][3];
 271         t[3] = v[3] - in->m[3][3];
 272         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0] + t[3] * in->m[3][0];
 273         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1] + t[3] * in->m[3][1];
 274         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2] + t[3] * in->m[3][2];
 275         out[3] = t[0] * in->m[0][3] + t[1] * in->m[1][3] + t[2] * in->m[2][3] + t[3] * in->m[3][3];
 276 }
 277
 278 // FIXME: optimize
 279 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
 280 {
 281         matrix4x4_t base, temp;
 282         Matrix4x4_Copy(out, &base);
 283         Matrix4x4_CreateTranslate(&temp, x, y, z);
 284         Matrix4x4_Concat(out, &base, &temp);
 285 }
 286
 287 // FIXME: optimize
 288 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 289 {
 290         matrix4x4_t base, temp;
 291         Matrix4x4_Copy(out, &base);
 292         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
 293         Matrix4x4_Concat(out, &base, &temp);
 294 }
 295
 296 // FIXME: optimize
 297 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
 298 {
 299         matrix4x4_t base, temp;
 300         Matrix4x4_Copy(out, &base);
 301         Matrix4x4_CreateScale(&temp, x);
 302         Matrix4x4_Concat(out, &base, &temp);
 303 }
 304
 305 // FIXME: optimize
 306 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
 307 {
 308         matrix4x4_t base, temp;
 309         Matrix4x4_Copy(out, &base);
 310         Matrix4x4_CreateScale3(&temp, x, y, z);
 311         Matrix4x4_Concat(out, &base, &temp);
 312 }
 313