Für die Vektorrechnung habe ich die Funktionen in einem Unterprogramm vektor.c zusammengefasst.

// Funktionen für die Vektor Rechnung 
//
// Matthias Busse 1.10.2018 Version 1.0

// Inhalt:
// vAdd(v1, v2, v3)           : v1+v2=v3
// vSub(v1, v2, v3)           : v1-v2=v3
// vMultF(v1, f, v2)          : v1*f=v2
// f=vBetrag(v1)              : |v1|=f
// f=vSkalarprodukt(v1, v2)   : v1 * v2=f
// f=vWinkelGrad(v1, v2)      : Winkel zwischen v1 und v2 in Grad
// vVektorprodukt(v1, v2, v3) : v1 x v2 = v3
// vEinheitsvektor(v1, v2)    : v1 / |v1| = v2, Länge=1
// vRichtungscosinusX(v1)     : v1 Winkel zur X-Achse in Grad
// vRichtungscosinusY(v1)     : v1 Winkel zur Y-Achse in Grad
// vRichtungscosinusZ(v1)     : v1 Winkel zur Z-Achse in Grad

//      z |
//        |
//        |
//        ---------- y
//       /
//    x /


void vAdd(float v1[3], float v2[3], float v3[3]) {
// Vektoren Addieren v1+v2=v3 
// Matthias Busse Version 1.0 vom 30.9.2018
  v3[0]=v1[0]+v2[0];
  v3[1]=v1[1]+v2[1];
  v3[2]=v1[2]+v2[2];
}

void vSub(float v1[3], float v2[3], float v3[3]) {
// Vektoren Subtrahieren v1-v2=v3 
// Matthias Busse Version 1.0 vom 30.9.2018
  v3[0]=v1[0]-v2[0];
  v3[1]=v1[1]-v2[1];
  v3[2]=v1[2]-v2[2];
}

void vMultF(float v1[3], float z, float v2[3]) {
// Vektor multiplizieren mit Zahl v1*z=v2 
// Matthias Busse Version 1.0 vom 30.9.2018
  v2[0]=v1[0]*z;
  v2[1]=v1[1]*z;
  v2[2]=v1[2]*z;
}

float vBetrag(float v1[3]) {
// Vektor Betrag (Länge) von v1 oder |v1|=z 
// Matthias Busse Version 1.0 vom 30.9.2018
  float z;
  z=sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]);
  return z;
}

float vSkalarprodukt(float v1[], float v2[]) {
// Vektoren Skalar multiplizieren v1 * v2 = z 
// |V1| * |V2| * cos winkel_dawischen = z
// Wenn das Ergebis 0 ist, dann sind die Vektoren senkrecht zueinander
// Matthias Busse Version 1.0 vom 30.9.2018
  float z;
  z= (v1[0]*v2[0]) + (v1[1]*v2[1]) + (v1[2]*v2[2]);
  return z;
}

float vWinkelGrad(float v1[], float v2[]) {
// Den Winkel in Grad zwischen zwei Vektoren berechnen  
// Matthias Busse Version 1.0 vom 30.9.2018
  float z, w;
  z=vSkalarprodukt(v1, v2);
  w = z / vBetrag(v1) / vBetrag(v2);
  return acos(w)*57.296; // Rad zu Grad
}

void vVektorprodukt(float v1[3], float v2[3], float v3[3]) {
// Vektorprodukt v1 x v2 = v3
// v3 steht senkrecht auf v1 und v2
// v3 Betrag |v3| ist die Fläche vom v1 und v2 Parallelogramm
// Winkel zwischen v1 und v2 ist 
// |v1 x v2| = |v3| = |A| * |B| * sin winkel_dawischen
// Matthias Busse Version 1.0 vom 30.9.2018
  v3[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
  v3[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
  v3[1]= -1.0 * ((v1[0]*v2[2]) - (v1[2]*v2[0]));
}

void vEinheitsvektor(float v1[3], float v2[3]) {
// Einheitsvektor v1 / |v1| = v2
// Ergibt den Vektor mit der Länge 1
// Matthias Busse Version 0.1 vom 01.10.2018
  float betrag;
  betrag=vBetrag(v1);
  v2[0]=v1[0]/betrag;
  v2[1]=v1[1]/betrag;
  v2[2]=v1[2]/betrag;
}

float vRichtungscosinusX(float v1[3]) {
// Der RichtungskosinusX bildet den Winkel zwischen dem Vektor und der X Achse
// cos(w) = vx / |v|
// Matthias Busse Version 1.0 vom 01.10.2018
  float winkel;
  winkel=acos(v1[0]/vBetrag(v1));
  return winkel*57.296; // Rad zu Grad
}

float vRichtungscosinusY(float v1[3]) {
// Der RichtungskosinusY bildet den Winkel zwischen dem Vektor und der Y Achse
// cos(w) = vy / |v|
// Matthias Busse Version 1.0 vom 01.10.2018
  float winkel;
  winkel=acos(v1[1]/vBetrag(v1));
  return winkel*57.296; // Rad zu Grad
}

float vRichtungscosinusZ(float v1[3]) {
// Der RichtungskosinusZ bildet den Winkel zwischen dem Vektor und der Z Achse
// cos(w) = vz / |v|
// Matthias Busse Version 1.0 vom 01.10.2018
  float winkel;
  winkel=acos(v1[2]/vBetrag(v1));
  return winkel*57.296; // Rad zu Grad
}

Hier das Programm um die Vektor Funktionen zu testen.
Das Unterprogramm wird mit #include „vektor.c“ eingebunden, da es im selben Verzeichnis liegt.
Die Ausgabe erfolgt auf der Konsole.

// Vektorrechnung, Funktionen Testprogramm
//
// Matthias Busse vom 1.10.2018

#include "vektor.c"

float va[3]={1,3,2}; // Vektor A mit x, y, z
float vb[3]={2,-1,4}; // x, y, z
float vc[3];
float f;

void setup() {
  Serial.begin(38400);
}

void loop() {
  Serial.print("Vektor A "); printVektor(va); 
  Serial.print("Vektor B "); printVektor(vb);
  Serial.println("Die Rechnungen :");
  vAdd(va, vb, vc);
  Serial.print(" A + B = "); printVektor(vc);
  vSub(va, vb, vc);
  Serial.print(" A - B = "); printVektor(vc);
  vMultF(va, 3.0, vc);
  Serial.print(" A * 3.0 = "); printVektor(vc);
  f=vBetrag(va);
  Serial.print("Betrag von A : |A| = "); Serial.println(f);
  f=vSkalarprodukt(va,vb);
  Serial.print("Skalarprodukt A * B = "); Serial.println(f);
  f=vWinkelGrad(va,vb);
  Serial.print("Winkel zwischen A und B = "); Serial.println(f);
  vVektorprodukt(va, vb, vc);
  Serial.print("Vektorprodukt A x B = "); printVektor(vc);
  vEinheitsvektor(va, vc);
  Serial.print("Einheitsvektor   "); printVektor(vc);
  f=vBetrag(vc); 
  Serial.print("Laenge = "); Serial.println(f);
  f=vRichtungscosinusX(va);
  Serial.print("Winkel A zur X-Achse = "); Serial.println(f);
  f=vRichtungscosinusY(va);
  Serial.print("Winkel A zur Y-Achse = "); Serial.println(f);
  f=vRichtungscosinusZ(va);
  Serial.print("Winkel A zur Z-Achse = "); Serial.println(f);
  Serial.print("Vektor A "); printVektor(va);
  Serial.println("-----------------------");
  delay(100000);
}

void printVektor(float v[]) {
  Serial.print("\tx:"); Serial.print(v[0]);
  Serial.print("\ty:"); Serial.print(v[1]);
  Serial.print("\tz:"); Serial.print(v[2]);
  Serial.println();
}

Und die Konsolen Ausgabe dazu

Vektor A 	x:1.00	y:3.00	z:2.00
Vektor B 	x:2.00	y:-1.00	z:4.00
Die Rechnungen :
 A + B = 	x:3.00	y:2.00	z:6.00
 A - B = 	x:-1.00	y:4.00	z:-2.00
 A * 3.0 = 	x:3.00	y:9.00	z:6.00
Betrag von A : |A| = 3.74
Skalarprodukt A * B = 7.00
Winkel zwischen A und B = 65.91
Vektorprodukt A x B = 	x:14.00	y:0.00	z:-7.00
Einheitsvektor   	x:0.27	y:0.80	z:0.53
Laenge = 1.00
Winkel A zur X-Achse = 74.50
Winkel A zur Y-Achse = 36.70
Winkel A zur Z-Achse = 57.69
Vektor A 	x:1.00	y:3.00	z:2.00
-----------------------

von Matthias Busse

Quelle: Wikipedia ,meine Formelsammlung und weitere.