Evan Brown / Mbed 2 deprecated APpart3_E_start_2

completed code

Dependencies:   mbed

quaternion.cpp@5:4af75af374cc, 2018-11-22 (annotated)

Committer:
evenbrownie
Date:
Thu Nov 22 22:40:15 2018 +0000
Revision:
5:4af75af374cc
Parent:
0:f43994f44684 
blah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
evenbrownie 0:f43994f44684 1 /*
evenbrownie 0:f43994f44684 2 * @author: Thomas Garcia, 2017
evenbrownie 0:f43994f44684 3 */
evenbrownie 0:f43994f44684 4
evenbrownie 0:f43994f44684 5 #include "quaternion.h"
evenbrownie 0:f43994f44684 6 #include <math.h>
evenbrownie 0:f43994f44684 7 #include <string.h>
evenbrownie 0:f43994f44684 8
evenbrownie 0:f43994f44684 9 float vector_normalize(struct vector *raw, struct vector *unit)
evenbrownie 0:f43994f44684 10 {
evenbrownie 0:f43994f44684 11 float mag = sqrt(pow(raw->x, 2) + pow(raw->y, 2) + pow(raw->z, 2));
evenbrownie 0:f43994f44684 12 if(mag != 0) {
evenbrownie 0:f43994f44684 13 unit->x = raw->x / mag;
evenbrownie 0:f43994f44684 14 unit->y = raw->y / mag;
evenbrownie 0:f43994f44684 15 unit->z = raw->z / mag;
evenbrownie 0:f43994f44684 16 }
evenbrownie 0:f43994f44684 17 return mag;
evenbrownie 0:f43994f44684 18 }
evenbrownie 0:f43994f44684 19
evenbrownie 0:f43994f44684 20 void vector_add(struct vector *v1, struct vector *v2, struct vector *result)
evenbrownie 0:f43994f44684 21 {
evenbrownie 0:f43994f44684 22 result->x = v1->x + v2->x;
evenbrownie 0:f43994f44684 23 result->y = v1->y + v2->y;
evenbrownie 0:f43994f44684 24 result->z = v1->z + v2->z;
evenbrownie 0:f43994f44684 25 }
evenbrownie 0:f43994f44684 26
evenbrownie 0:f43994f44684 27 void vector_multiply(struct vector *v, float c, struct vector *result)
evenbrownie 0:f43994f44684 28 {
evenbrownie 0:f43994f44684 29 result->x = v->x * c;
evenbrownie 0:f43994f44684 30 result->y = v->y * c;
evenbrownie 0:f43994f44684 31 result->z = v->z * c;
evenbrownie 0:f43994f44684 32 }
evenbrownie 0:f43994f44684 33
evenbrownie 0:f43994f44684 34 void quaternion_create(struct vector *v, float angle, struct quaternion *result)
evenbrownie 0:f43994f44684 35 {
evenbrownie 0:f43994f44684 36 result->r = cos(angle/2);
evenbrownie 0:f43994f44684 37 result->i = v->x * sin(angle/2);
evenbrownie 0:f43994f44684 38 result->j = v->y * sin(angle/2);
evenbrownie 0:f43994f44684 39 result->k = v->z * sin(angle/2);
evenbrownie 0:f43994f44684 40 }
evenbrownie 0:f43994f44684 41
evenbrownie 0:f43994f44684 42 void quaternion_rotate(struct vector *v, struct quaternion *q, struct vector *result)
evenbrownie 0:f43994f44684 43 {
evenbrownie 0:f43994f44684 44 struct vector n;
evenbrownie 0:f43994f44684 45 memcpy(&n, v, sizeof(n));
evenbrownie 0:f43994f44684 46 result->x = n.x * (1 - 2*(pow(q->j, 2) + pow(q->k, 2))) + n.y * 2*(q->i * q->j - q->k * q->r) + n.z * 2*(q->i * q->k + q->j * q->r);
evenbrownie 0:f43994f44684 47 result->y = n.x * 2*(q->i * q->j + q->k * q->r) + n.y * (1 - 2*(pow(q->i, 2) + pow(q->k, 2))) + n.z * 2*(q->j * q->k - q->i * q->r);
evenbrownie 0:f43994f44684 48 result->z = n.x * 2*(q->i * q->k - q->j * q->r) + n.y * 2*(q->j * q->k + q->i * q->r) + n.z * (1 - 2*(pow(q->i, 2) + pow(q->j, 2)));
evenbrownie 0:f43994f44684 49 }
evenbrownie 0:f43994f44684 50
evenbrownie 0:f43994f44684 51 void quaternion_multiply(struct quaternion *q1, struct quaternion *q2, struct quaternion *result)
evenbrownie 0:f43994f44684 52 {
evenbrownie 0:f43994f44684 53 result->r = q1->r * q2->r - q1->i * q2->i - q1->j * q2->j - q1->k * q2->k;
evenbrownie 0:f43994f44684 54 result->i = q1->r * q2->i + q1->i * q2->r + q1->j * q2->k - q1->k * q2->j;
evenbrownie 0:f43994f44684 55 result->j = q1->r * q2->j - q1->i * q2->k + q1->j * q2->r + q1->k * q2->i;
evenbrownie 0:f43994f44684 56 result->k = q1->r * q2->k + q1->i * q2->j - q1->j * q2->i + q1->k * q2->r;
evenbrownie 0:f43994f44684 57 }
evenbrownie 0:f43994f44684 58
evenbrownie 0:f43994f44684 59 float vector_roll(struct vector *v)
evenbrownie 0:f43994f44684 60 {
evenbrownie 0:f43994f44684 61 return atan2(v->x, sqrt(pow(v->z, 2) + pow(v->y, 2)));
evenbrownie 0:f43994f44684 62 }
evenbrownie 0:f43994f44684 63
evenbrownie 0:f43994f44684 64 float vector_pitch(struct vector *v)
evenbrownie 0:f43994f44684 65 {
evenbrownie 0:f43994f44684 66 return -atan2(v->y, v->z);
evenbrownie 0:f43994f44684 67 }
evenbrownie 0:f43994f44684 68