g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

now it compiles

eulerConversion
Martin 2022-03-04 22:05:04 +01:00
parent 5d24919125
commit f3dfbe4958
1 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
osc32bt/osc32bt.ino
View File

@ -91,6 +91,38 @@ OSCMessage emsg("/error/");
OSCMessage kmsg("/keys/");
OSCMessage quaternionMessage("/quaternion/");
OSCMessage quaternionDiffMessage("/quaternionDiff/");
OSCMessage eulerDiffMessage("eulerDiff");
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
BLA::Matrix<4,4> rotationMatrix = {
1 - (yy + zz), xy + wz, xz - wy, 0,
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
0, 0, 0, 1
};
//TODO: test whether BLA library uses column-major matrix notation in code
BLA::Matrix<3> eulerVector;
eulerVector.Fill(0);
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
} else {
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
eulerVector(2) = 0;
}
return eulerVector;
}
float clamp(float value,float min,float max) {
return fmaxf( min, fminf(max, value));
}
void setup() {
Wire.begin();
@ -313,33 +345,3 @@ void loop() {
}
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
BLA::Matrix<4,4> rotationMatrix = {
1 - (yy + zz), xy + wz, xz - wy, 0,
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
0, 0, 0, 1
};
//TODO: test whether BLA library uses column-major matrix notation in code
BLA::Matrix<3> eulerVector;
eulerVector.Fill(0);
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
} else {
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
eulerVector(2) = 0;
}
return eulerVector;
}
float clamp(float value,float min,float max) {
return fmaxf( min, fminf(max, value));
}