Improve vector and quaternion libraries

Make the order or basic methods consistent between Vector and Quaternion.
Remove `ZYX` from Euler method names as this is standard for robotics.
Rename angular rates to rotation vector, which is more correct.
Make rotation methods static, to keep the arguments order consistent.
Make `Quaternion::fromAxisAngle` accept Vector for axis.
Minor fixes.

flix/cli.ino

@@ -101,7 +101,7 @@ void doCommand(String str, bool echo = false) {
 		print("Loop rate: %.0f\n", loopRate);
 		print("dt: %f\n", dt);
 	} else if (command == "ps") {
-		Vector a = attitude.toEulerZYX();
+		Vector a = attitude.toEuler();
 		print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
 	} else if (command == "psq") {
 		print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);

flix/control.ino

@@ -95,7 +95,7 @@ void interpretRC() {
 	} else if (mode == STAB) {
 		yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE;
 
-		attitudeTarget = Quaternion::fromEulerZYX(Vector(
+		attitudeTarget = Quaternion::fromEuler(Vector(
 			controls[rollChannel] * tiltMax,
 			controls[pitchChannel] * tiltMax,
 			attitudeTarget.getYaw()));
@@ -122,10 +122,10 @@ void controlAttitude() {
 	}
 
 	const Vector up(0, 0, 1);
-	Vector upActual = attitude.rotateVector(up);
-	Vector upTarget = attitudeTarget.rotateVector(up);
+	Vector upActual = Quaternion::rotateVector(up, attitude);
+	Vector upTarget = Quaternion::rotateVector(up, attitudeTarget);
 
-	Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
+	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
 	ratesTarget.x = rollPID.update(error.x, dt);
 	ratesTarget.y = pitchPID.update(error.y, dt);

flix/estimate.ino

@@ -23,7 +23,7 @@ void applyGyro() {
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude
-	attitude = attitude.rotate(Quaternion::fromAngularRates(rates * dt));
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
 }
 
 void applyAcc() {
@@ -34,9 +34,9 @@ void applyAcc() {
 	if (!landed) return;
 
 	// calculate accelerometer correction
-	Vector up = attitude.rotateVector(Vector(0, 0, 1));
-	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * WEIGHT_ACC;
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
 
 	// apply correction
-	attitude = attitude.rotate(Quaternion::fromAngularRates(correction));
+	attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
 }

flix/log.ino

@@ -41,8 +41,8 @@ float logBuffer[LOG_SIZE][logColumns];
 
 void prepareLogData() {
 	tFloat = t;
-	attitudeEuler = attitude.toEulerZYX();
-	attitudeTargetEuler = attitudeTarget.toEulerZYX();
+	attitudeEuler = attitude.toEuler();
+	attitudeTargetEuler = attitudeTarget.toEuler();
 }
 
 void logData() {

flix/quaternion.h

@@ -15,22 +15,22 @@ public:
 
 	Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};
 
-	static Quaternion fromAxisAngle(float a, float b, float c, float angle) {
+	static Quaternion fromAxisAngle(const Vector& axis, float angle) {
 		float halfAngle = angle * 0.5;
 		float sin2 = sin(halfAngle);
 		float cos2 = cos(halfAngle);
-		float sinNorm = sin2 / sqrt(a * a + b * b + c * c);
-		return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
+		float sinNorm = sin2 / axis.norm();
+		return Quaternion(cos2, axis.x * sinNorm, axis.y * sinNorm, axis.z * sinNorm);
 	}
 
-	static Quaternion fromAngularRates(const Vector& rates) {
-		if (rates.zero()) {
+	static Quaternion fromRotationVector(const Vector& rotation) {
+		if (rotation.zero()) {
 			return Quaternion();
 		}
-		return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
+		return Quaternion::fromAxisAngle(rotation, rotation.norm());
 	}
 
-	static Quaternion fromEulerZYX(const Vector& euler) {
+	static Quaternion fromEuler(const Vector& euler) {
 		float cx = cos(euler.x / 2);
 		float cy = cos(euler.y / 2);
 		float cz = cos(euler.z / 2);
@@ -60,14 +60,37 @@ public:
 		return ret;
 	}
 
-	void toAxisAngle(float& a, float& b, float& c, float& angle) const {
-		angle = acos(w) * 2;
-		a = x / sin(angle / 2);
-		b = y / sin(angle / 2);
-		c = z / sin(angle / 2);
+	bool finite() const {
+		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
-	Vector toEulerZYX() const {
+	float norm() const {
+		return sqrt(w * w + x * x + y * y + z * z);
+	}
+
+	void normalize() {
+		float n = norm();
+		w /= n;
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	void toAxisAngle(Vector& axis, float& angle) const {
+		angle = acos(w) * 2;
+		axis.x = x / sin(angle / 2);
+		axis.y = y / sin(angle / 2);
+		axis.z = z / sin(angle / 2);
+	}
+
+	Vector toRotationVector() const {
+		float angle;
+		Vector axis;
+		toAxisAngle(axis, angle);
+		return angle * axis;
+	}
+
+	Vector toEuler() const {
 		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
 		Vector euler;
 		float sqx = x * x;
@@ -112,18 +135,9 @@ public:
 
 	void setYaw(float yaw) {
 		// TODO: optimize?
-		Vector euler = toEulerZYX();
+		Vector euler = toEuler();
 		euler.z = yaw;
-		(*this) = Quaternion::fromEulerZYX(euler);
-	}
-
-	Quaternion& operator *= (const Quaternion& q) {
-		Quaternion ret(
-			w * q.w - x * q.x - y * q.y - z * q.z,
-			w * q.x + x * q.w + y * q.z - z * q.y,
-			w * q.y + y * q.w + z * q.x - x * q.z,
-			w * q.z + z * q.w + x * q.y - y * q.x);
-		return (*this = ret);
+		(*this) = Quaternion::fromEuler(euler);
 	}
 
 	Quaternion operator * (const Quaternion& q) const {
@@ -143,18 +157,6 @@ public:
 			-z * normSqInv);
 	}
 
-	float norm() const {
-		return sqrt(w * w + x * x + y * y + z * z);
-	}
-
-	void normalize() {
-		float n = norm();
-		w /= n;
-		x /= n;
-		y /= n;
-		z /= n;
-	}
-
 	Vector conjugate(const Vector& v) const {
 		Quaternion qv(0, v.x, v.y, v.z);
 		Quaternion res = (*this) * qv * inversed();
@@ -167,22 +169,27 @@ public:
 		return Vector(res.x, res.y, res.z);
 	}
 
-	// Rotate vector by quaternion
-	Vector rotateVector(const Vector& v) const {
-		return conjugateInversed(v);
-	}
-
 	// Rotate quaternion by quaternion
-	Quaternion rotate(const Quaternion& q, const bool normalize = true) const {
-		Quaternion rotated = (*this) * q;
+	static Quaternion rotate(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
+		Quaternion rotated = a * b;
 		if (normalize) {
 			rotated.normalize();
 		}
 		return rotated;
 	}
 
-	bool finite() const {
-		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
+	// Rotate vector by quaternion
+	static Vector rotateVector(const Vector& v, const Quaternion& q) {
+		return q.conjugateInversed(v);
+	}
+
+	// Quaternion between two quaternions a and b
+	static Quaternion between(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
+		Quaternion q = a * b.inversed();
+		if (normalize) {
+			q.normalize();
+		}
+		return q;
 	}
 
 	size_t printTo(Print& p) const {

flix/vector.h

@@ -13,14 +13,18 @@ public:
 
 	Vector(float x, float y, float z): x(x), y(y), z(z) {};
 
-	float norm() const {
-		return sqrt(x * x + y * y + z * z);
-	}
-
 	bool zero() const {
 		return x == 0 && y == 0 && z == 0;
 	}
 
+	bool finite() const {
+		return isfinite(x) && isfinite(y) && isfinite(z);
+	}
+
+	float norm() const {
+		return sqrt(x * x + y * y + z * z);
+	}
+
 	void normalize() {
 		float n = norm();
 		x /= n;
@@ -74,10 +78,6 @@ public:
 		return !(*this == b);
 	}
 
-	bool finite() const {
-		return isfinite(x) && isfinite(y) && isfinite(z);
-	}
-
 	static float dot(const Vector& a, const Vector& b) {
 		return a.x * b.x + a.y * b.y + a.z * b.z;
 	}
@@ -86,18 +86,18 @@ public:
 		return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
 	}
 
-	static float angleBetweenVectors(const Vector& a, const Vector& b) {
+	static float angleBetween(const Vector& a, const Vector& b) {
 		return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
 	}
 
-	static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) {
+	static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
 		Vector direction = cross(a, b);
 		if (direction.zero()) {
 			// vectors are opposite, return any perpendicular vector
 			return cross(a, Vector(1, 0, 0));
 		}
 		direction.normalize();
-		float angle = angleBetweenVectors(a, b);
+		float angle = angleBetween(a, b);
 		return direction * angle;
 	}