Я не могу найти никаких проблем с кодом, размещенным, и предположил бы, что проблема в другом месте.Если это поможет, мой анализ опубликованного кода таков:
Первые шесть строк, касающиеся _geomagnetic 0–5, влияют на очень простой низкочастотный фильтр, который предполагает, что вы вызываете метод через регулярные промежутки времени.Таким образом, вы получите версию вектора магнитометра, надеюсь, с устранением высокочастотного джиттера.
bzero обнуляет результат, готов к накоплению.
Строки до объявления и присвоенияГц возьмите векторы магнитометра и акселерометра и выполните перекрестное произведение.Таким образом, H (x, y, z) теперь является вектором под прямыми углами как к акселерометру (который считается «вниз»), так и к магнитометру (который будет вперед + немного вверх).Назовите это побочным вектором.
Вещи invH и invA, вплоть до умножения Az на invA, гарантируют, что векторы стороны и акселерометра / вниз имеют единичную длину.
M (x,Затем создается y, z) как перекрестное произведение боковых и нисходящих векторов (т. е. вектора под прямым углом к обоим из них).Таким образом, он дает фронтальный вектор.
Наконец, три вектора используются для заполнения матрицы, используя тот факт, что обратная сторона ортонормированной матрицы 3x3 является ее транспонированной (хотя это своего рода скрыто, кстати,вещи выложены - обратите внимание на индексы массива).Вы фактически устанавливаете все в матрице напрямую, поэтому bzero не был необходим в терминах чистого результата.
glLoadMatrixf
- это то, что нужно использовать правильно, потому что именно так вы умножаетесь на произвольную матрицу мажорных столбцов вOpenGL ES 1.x.