1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
|
#include "vector.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <assert.h>
static inline vector_t *vector_exists(vector_t *v)
{
assert(v);
return v;
}
// Overwrites stuff in p. If p is null a new vector is created
vector_t *vector_set(vector_t *p, COORD_T x, COORD_T y, COORD_T z)
{
p = vector_exists(p);
p->x = x;
p->y = y;
p->z = z;
return p;
}
// Can also be used to create a vector by leaving v NULL
vector_t *vector_copy(vector_t *dest, vector_t *src)
{
dest = vector_exists(dest);
// If v exist copy it over
if (src) {
memcpy(dest, src, sizeof(vector_t));
}
return dest;
}
COORD_T vector_len(vector_t *v)
{
return sqrt( v->x * v->x + v->y * v->y + v->z * v->z );
}
vector_t *vector_add(vector_t *dest, vector_t *a, vector_t *b)
{
dest = vector_exists(dest);
dest->x = a->x + b->x;
dest->y = a->y + b->y;
dest->z = a->z + b->z;
return dest;
}
vector_t *vector_sub(vector_t *dest, vector_t *a, vector_t *b)
{
dest = vector_exists(dest);
dest->x = a->x - b->x;
dest->y = a->y - b->y;
dest->z = a->z - b->z;
return dest;
}
vector_t *vector_mult(vector_t *dest, vector_t *a, vector_t *b)
{
dest = vector_exists(dest);
dest->x = a->x * b->x;
dest->y = a->y * b->y;
dest->z = a->z * b->z;
return dest;
}
vector_t *vector_scale(vector_t *dest, vector_t *a, COORD_T b)
{
dest = vector_exists(dest);
dest->x = a->x * b;
dest->y = a->y * b;
dest->z = a->z * b;
return dest;
}
vector_t *vector_scale_inv(vector_t *dest, vector_t *a, COORD_T b)
{
dest = vector_exists(dest);
dest->x = a->x / b;
dest->y = a->y / b;
dest->z = a->z / b;
return dest;
}
COORD_T vector_dot(vector_t *a, vector_t *b)
{
return a->x * b->x + a->y * b->y + a->z * b->z;
}
vector_t *vector_cross(vector_t *dest, vector_t *a, vector_t *b)
{
dest = vector_exists(dest);
dest->x = a->y * b->z - a->z * b->y;
dest->y = a->z * b->x - a->x * b->z;
dest->z = a->x * b->y - a->y * b->x;
return dest;
}
void vector_print(vector_t *v)
{
Serial.println("Not implemented");
}
|
