Stilllegung des Forums
Das Forum wurde am 05.06.2023 nach über 20 Jahren stillgelegt (weitere Informationen und ein kleiner Rückblick).
Registrierungen, Anmeldungen und Postings sind nicht mehr möglich. Öffentliche Inhalte sind weiterhin zugänglich.
Das Team von spieleprogrammierer.de bedankt sich bei der Community für die vielen schönen Jahre.
Wenn du eine deutschsprachige Spieleentwickler-Community suchst, schau doch mal im Discord und auf ZFX vorbei!
Werbeanzeige
Administrator
Ist hier vielleicht was für dich dabei?
https://www.google.com/search?q=kalman+f…chrome&ie=UTF-8
Administrator
C-/C++-Quelltext |
|
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 |
struct EulerAngles { double roll, pitch, yaw; }; struct Quaternion { double w, x, y, z; }; Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X) { // Abbreviations for the various angular functions double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); double sp = sin(pitch * 0.5); double cr = cos(roll * 0.5); double sr = sin(roll * 0.5); Quaternion q; q.w = cy * cp * cr + sy * sp * sr; q.x = cy * cp * sr - sy * sp * cr; q.y = sy * cp * sr + cy * sp * cr; q.z = sy * cp * cr - cy * sp * sr; return q; } EulerAngles ToEulerAngles(Quaternion q) { EulerAngles angles; // roll (x-axis rotation) double sinr_cosp = 2 * (q.w * q.x + q.y * q.z); double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); angles.roll = std::atan2(sinr_cosp, cosr_cosp); // pitch (y-axis rotation) double sinp = 2 * (q.w * q.y - q.z * q.x); if (std::abs(sinp) >= 1) angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range else angles.pitch = std::asin(sinp); // yaw (z-axis rotation) double siny_cosp = 2 * (q.w * q.z + q.x * q.y); double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); angles.yaw = std::atan2(siny_cosp, cosy_cosp); return angles; } |
Werbeanzeige