src/math/point.c

   1 #include <assert.h>
   2 #include <math.h>
   3
   4 #include "point.h"
   5
   6 vec_t vec(float x, float y)
   7 {
   8     vec_t result = {
   9         .x = x,
  10         .y = y
  11     };
  12     return result;
  13 }
  14
  15 vec_t vec_from_polar(float arg, float mag)
  16 {
  17     return vec_scala_mult(
  18         vec(cosf(arg), sinf(arg)),
  19         mag);
  20 }
  21
  22 vec_t vec_from_ps(point_t p1, point_t p2)
  23 {
  24     vec_t result = {
  25         .x = p2.x - p1.x,
  26         .y = p2.y - p1.y
  27     };
  28     return result;
  29 }
  30
  31 float vec_arg(vec_t v)
  32 {
  33     return atan2f(v.y, v.x);
  34 }
  35
  36 float vec_mag(vec_t v)
  37 {
  38     return sqrtf(v.x * v.x + v.y * v.y);
  39 }
  40
  41 vec_t vec_sum(vec_t v1, vec_t v2)
  42 {
  43     vec_t result = {
  44         .x = v1.x + v2.x,
  45         .y = v1.y + v2.y
  46     };
  47     return result;
  48 }
  49
  50 vec_t vec_neg(vec_t v)
  51 {
  52     vec_t result = {
  53         .x = -v.x,
  54         .y = -v.y
  55     };
  56
  57     return result;
  58 }
  59
  60 float vec_length(vec_t v)
  61 {
  62     return sqrtf(v.x * v.x + v.y * v.y);
  63 }
  64
  65 void vec_add(vec_t *v1, vec_t v2)
  66 {
  67     v1->x += v2.x;
  68     v1->y += v2.y;
  69 }
  70
  71 vec_t vec_scala_mult(vec_t v, float scalar)
  72 {
  73     vec_t result = {
  74         .x = v.x * scalar,
  75         .y = v.y * scalar
  76     };
  77     return result;
  78 }
  79
  80 vec_t vec_entry_mult(vec_t v1, vec_t v2)
  81 {
  82     vec_t result = {
  83         .x = v1.x * v2.x,
  84         .y = v1.y * v2.y
  85     };
  86
  87     return result;
  88 }
  89
  90 vec_t vec_entry_div(vec_t v1, vec_t v2)
  91 {
  92     vec_t result = {
  93         .x = v1.x / v2.x,
  94         .y = v1.y / v2.y
  95     };
  96
  97     return result;
  98 }
  99
 100 float rad_to_deg(float a)
 101 {
 102     return 180 / PI * a;
 103 }
 104
 105 point_t point_mat3x3_product(point_t p, mat3x3 m)
 106 {
 107     /* Convert p to Homogeneous coordinates */
 108     const float homo_p[3] = {p.x, p.y, 1};
 109
 110     /* Transform p with matrix m */
 111     const float trans_p[3] = {
 112         homo_p[0] * m.M[0][0] + homo_p[1] * m.M[0][1] + homo_p[2] * m.M[0][2],
 113         homo_p[0] * m.M[1][0] + homo_p[1] * m.M[1][1] + homo_p[2] * m.M[1][2],
 114         homo_p[0] * m.M[2][0] + homo_p[1] * m.M[2][1] + homo_p[2] * m.M[2][2]
 115     };
 116
 117     /* Convert p back to Cartesian coordinates */
 118     const point_t result_p = {
 119         .x = trans_p[0] / trans_p[2],
 120         .y = trans_p[1] / trans_p[2]
 121     };
 122
 123     return result_p;
 124 }