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         c = cos(angle);
 102         s = sin(angle);
 103
 104         out->m[0][0]=x * x + c * (1 - x * x);
 105         out->m[0][1]=x * y * (1 - c) + z * s;
 106         out->m[0][2]=z * x * (1 - c) - y * s;
 107         out->m[0][3]=0.0f;
 108         out->m[1][0]=x * y * (1 - c) - z * s;
 109         out->m[1][1]=y * y + c * (1 - y * y);
 110         out->m[1][2]=y * z * (1 - c) + x * s;
 111         out->m[1][3]=0.0f;
 112         out->m[2][0]=z * x * (1 - c) + y * s;
 113         out->m[2][1]=y * z * (1 - c) - x * s;
 114         out->m[2][2]=z * z + c * (1 - z * z);
 115         out->m[2][3]=0.0f;
 116         out->m[3][0]=0.0f;
 117         out->m[3][1]=0.0f;
 118         out->m[3][2]=0.0f;
 119         out->m[3][3]=1.0f;
 120 }
 121
 122 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
 123 {
 124         out->m[0][0]=x;
 125         out->m[0][1]=0.0f;
 126         out->m[0][2]=0.0f;
 127         out->m[0][3]=0.0f;
 128         out->m[1][0]=0.0f;
 129         out->m[1][1]=x;
 130         out->m[1][2]=0.0f;
 131         out->m[1][3]=0.0f;
 132         out->m[2][0]=0.0f;
 133         out->m[2][1]=0.0f;
 134         out->m[2][2]=x;
 135         out->m[2][3]=0.0f;
 136         out->m[3][0]=0.0f;
 137         out->m[3][1]=0.0f;
 138         out->m[3][2]=0.0f;
 139         out->m[3][3]=1.0f;
 140 }
 141
 142 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
 143 {
 144         out->m[0][0]=x;
 145         out->m[0][1]=0.0f;
 146         out->m[0][2]=0.0f;
 147         out->m[0][3]=0.0f;
 148         out->m[1][0]=0.0f;
 149         out->m[1][1]=y;
 150         out->m[1][2]=0.0f;
 151         out->m[1][3]=0.0f;
 152         out->m[2][0]=0.0f;
 153         out->m[2][1]=0.0f;
 154         out->m[2][2]=z;
 155         out->m[2][3]=0.0f;
 156         out->m[3][0]=0.0f;
 157         out->m[3][1]=0.0f;
 158         out->m[3][2]=0.0f;
 159         out->m[3][3]=1.0f;
 160 }
 161
 162 void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
 163 {
 164         vx[0] = in->m[0][0];
 165         vx[1] = in->m[1][0];
 166         vx[2] = in->m[2][0];
 167         vy[0] = in->m[0][1];
 168         vy[1] = in->m[1][1];
 169         vy[2] = in->m[2][1];
 170         vz[0] = in->m[0][2];
 171         vz[1] = in->m[1][2];
 172         vz[2] = in->m[2][2];
 173         t[0] = in->m[0][3];
 174         t[1] = in->m[1][3];
 175         t[2] = in->m[2][3];
 176 }
 177
 178 void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t)
 179 {
 180         vx[0] = in->m[0][0];
 181         vx[1] = in->m[1][0];
 182         vx[2] = in->m[2][0];
 183         vx[3] = in->m[3][0];
 184         vy[0] = in->m[0][1];
 185         vy[1] = in->m[1][1];
 186         vy[2] = in->m[2][1];
 187         vy[3] = in->m[3][1];
 188         vz[0] = in->m[0][2];
 189         vz[1] = in->m[1][2];
 190         vz[2] = in->m[2][2];
 191         vz[3] = in->m[3][2];
 192         t[0] = in->m[0][3];
 193         t[1] = in->m[1][3];
 194         t[2] = in->m[2][3];
 195         t[3] = in->m[3][3];
 196 }
 197
 198 void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
 199 {
 200         out->m[0][0] = vx[0];
 201         out->m[0][1] = vy[0];
 202         out->m[0][2] = vz[0];
 203         out->m[0][3] = t[0];
 204         out->m[1][0] = vx[1];
 205         out->m[1][1] = vy[1];
 206         out->m[1][2] = vz[1];
 207         out->m[1][3] = t[1];
 208         out->m[2][0] = vx[2];
 209         out->m[2][1] = vy[2];
 210         out->m[2][2] = vz[2];
 211         out->m[2][3] = t[2];
 212         out->m[3][0] = 0.0f;
 213         out->m[3][1] = 0.0f;
 214         out->m[3][2] = 0.0f;
 215         out->m[3][3] = 1.0f;
 216 }
 217
 218 void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t)
 219 {
 220         out->m[0][0] = vx[0];
 221         out->m[0][1] = vy[0];
 222         out->m[0][2] = vz[0];
 223         out->m[0][3] = t[0];
 224         out->m[1][0] = vx[1];
 225         out->m[1][1] = vy[1];
 226         out->m[1][2] = vz[1];
 227         out->m[1][3] = t[1];
 228         out->m[2][0] = vx[2];
 229         out->m[2][1] = vy[2];
 230         out->m[2][2] = vz[2];
 231         out->m[2][3] = t[2];
 232         out->m[3][0] = vx[3];
 233         out->m[3][1] = vy[3];
 234         out->m[3][2] = vz[3];
 235         out->m[3][3] = t[3];
 236 }
 237
 238 void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
 239 {
 240         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 241         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 242         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 243 }
 244
 245 void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
 246 {
 247         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];
 248         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];
 249         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];
 250         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];
 251 }
 252
 253 void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
 254 {
 255         vec3_t t;
 256         t[0] = v[0] - in->m[0][3];
 257         t[1] = v[1] - in->m[1][3];
 258         t[2] = v[2] - in->m[2][3];
 259         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 260         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 261         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 262 }
 263
 264 void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
 265 {
 266         vec3_t t;
 267         t[0] = v[0] - in->m[0][3];
 268         t[1] = v[1] - in->m[1][3];
 269         t[2] = v[2] - in->m[2][3];
 270         t[3] = v[3] - in->m[3][3];
 271         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];
 272         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];
 273         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];
 274         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];
 275 }
 276
 277 // FIXME: optimize
 278 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
 279 {
 280         matrix4x4_t base, temp;
 281         Matrix4x4_Copy(out, &base);
 282         Matrix4x4_CreateTranslate(&temp, x, y, z);
 283         Matrix4x4_Concat(out, &base, &temp);
 284 }
 285
 286 // FIXME: optimize
 287 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 288 {
 289         matrix4x4_t base, temp;
 290         Matrix4x4_Copy(out, &base);
 291         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
 292         Matrix4x4_Concat(out, &base, &temp);
 293 }
 294
 295 // FIXME: optimize
 296 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
 297 {
 298         matrix4x4_t base, temp;
 299         Matrix4x4_Copy(out, &base);
 300         Matrix4x4_CreateScale(&temp, x);
 301         Matrix4x4_Concat(out, &base, &temp);
 302 }
 303
 304 // FIXME: optimize
 305 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
 306 {
 307         matrix4x4_t base, temp;
 308         Matrix4x4_Copy(out, &base);
 309         Matrix4x4_CreateScale3(&temp, x, y, z);
 310         Matrix4x4_Concat(out, &base, &temp);
 311 }
 312