Ищете лучший способ делать Quatterion differentiaton

Question

У меня есть кватернион (4x1) и вектор угловой скорости (3x1), и я вызываю функцию для вычисления дифференциального кватерниона, как объяснено в этом web. Код выглядит следующим образом:

float wx = w.at<float>(0); 
float wy = w.at<float>(1); 
float wz = w.at<float>(2); 
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0); 
float qy = q.at<float>(1); 
float qz = q.at<float>(2); 

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy); // qdiffx 
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz); // qdiffy 
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx); // qdiffz 
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz); // qdiffw 

Так что теперь у меня есть дифференциальное кватернион, хранящееся в д, а затем я обновить кватернион на просто добавив этот дифференциал кватернионов.

Этот метод подходит для прогнозирования движения жестких объектов или есть лучший способ предсказать кватернион с угловой скоростью? Это работает, но я не получаю ожидаемых результатов.

Answer 1

Есть несколько вещей, которые могут произойти. Вы не говорите о нормализации этого кватерниона. Плохие вещи обязательно произойдут, если вы этого не сделаете. Вы также не говорите, что умножаете компоненты дельта-кватернионов на количество времени, прошедшее с dt, прежде чем добавлять их в исходный кватернион. Если ваша угловая скорость находится в радианах в секунду, но вы только шагнули вперед на долю секунды, вы зашли слишком далеко. Однако даже в этом случае, поскольку вы проходите через определенное количество времени и пытаетесь притвориться, что это бесконечно мало, странные вещи произойдут, особенно если ваша временная или угловая скорость велика.

Физический двигатель, ODE, предоставляет возможность обновлять вращение тела от его угловой скорости, как если бы он выполнял бесконечно малый шаг или обновлялся с использованием шага конечного размера. Конечный шаг намного точнее, но включает в себя некоторые триггеры. функций и, следовательно, немного медленнее. Соответствующий исходный код ODE можно увидеть here, lines 300-321, код которого определяет дельта-кватернион here, line 310.

float wMag = sqrt(wx*wx + wy*wy + wz*wz); 
float theta = 0.5f*wMag*dt; 
q[0] = cos(theta); // Scalar component 
float s = sinc(theta)*0.5f*dt; 
q[1] = wx * s; 
q[2] = wy * s; 
q[3] = wz * s; 

Где sinc(x) является:

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667); 
else return sin(x)/x; 

Это позволяет избежать проблемы деления на ноль и до сих пор очень точным.

Затем кватернион q предварительно умножен на существующее представление кватерниона ориентации тела. Затем, повторно нормализовать.

---

Редактировать - Где эта формула приходит от:

Рассмотрим начальную кватернион q0 и окончательное кватернион q1, что приводит после того, как вращающийся с угловой скоростью w для dt количества времени. Все, что мы делаем здесь, - это изменение вектора угловой скорости в кватернион, а затем поворот первой ориентации на этот кватернион. Как кватернионы, так и угловые скорости являются вариациями в представлении по оси. Тело, которое повернуто от его канонической ориентации на theta вокруг оси единицы [x,y,z] будет иметь следующее кватернионное представление своей ориентации: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]. Корпус Вращающийсяtheta/s вокруг оси устройства [x,y,z] будет иметь угловую скорость w=[theta*x theta*y theta*z]. Итак, чтобы решить, сколько вращений произойдет за dt секунд, мы сначала извлекаем величину угловой скорости: theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2). Затем мы находим фактический угол, умножая на dt (и одновременно разделим на 2 для удобства превращения этого в кватернион).Так как нам нужно нормализовать ось [x y z], мы также разделим на theta. Вот откуда берется часть sinc(theta). (так как theta имеет дополнительный 0.5*dt в нем от величины, мы умножаем это обратно). Функция sinc(x) просто использует приближение серии Taylor для функции, когда x невелика, поскольку она является численно стабильной и более точной для этого. Способность использовать эту удобную функцию - это то, почему мы не просто разделили фактическую величину wMag. Тела, которые не вращаются очень быстро, будут иметь очень малые угловые скорости. Поскольку мы ожидаем, что это будет довольно распространено, нам нужно стабильное решение. В результате мы получаем кватернион, который представляет собой шаг шага в один шаг dt вращения.

Answer 2

Существует Metod с очень хорошим компромиссом между скоростью и точностью, как приращения quaterniom, представляющим состоянием вращения (т.е. интегрировать дифференциальное уравнение вращательного движения) с помощью небольшого вектора приращения угла dphi (который является вектором угловая скорость omega mulptipliad по времени шаг dt).

Exact (и медленный) метод вращения кватернион вектором:

void rotate_quaternion_by_vector_vec (double [] dphi, double [] q) { 
    double x = dphi[0]; 
    double y = dphi[1]; 
    double z = dphi[2]; 

    double r2 = x*x + y*y + z*z; 
    double norm = Math.sqrt(r2); 

    double halfAngle = norm * 0.5d; 
    double sa = Math.sin(halfAngle)/norm; // we normalize it here to save multiplications 
    double ca = Math.cos(halfAngle); 
    x*=sa; y*=sa; z*=sa; 

    double qx = q[0]; 
    double qy = q[1]; 
    double qz = q[2]; 
    double qw = q[3]; 

    q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 
} 

Проблема заключается в том, что вы должны вычислить медленные функции, такие как cos, sin, sqrt. Вместо этого вы можете получить значительное увеличение скорости и разумную точность при малых углах (что имеет место, если временной шаг вашей симуляции достаточно мал) путем аппроксимации sin и cos с помощью расширения taylor, выраженного только с использованием norm^2 вместо norm.

Как это метод быстрого вращения кватернион по вектору:

void rotate_quaternion_by_vector_Fast (double [] dphi, double [] q) { 
    double x = dphi[0]; 
    double y = dphi[1]; 
    double z = dphi[2]; 

    double r2 = x*x + y*y + z*z; 

    // derived from second order taylor expansion 
    // often this is accuracy is sufficient 
    final double c3 = 1.0d/(6 * 2*2*2)  ; // evaulated in compile time 
    final double c2 = 1.0d/(2 * 2*2)   ; // evaulated in compile time 
    double sa = 0.5d - c3*r2    ; 
    double ca = 1 - c2*r2    ; 

    x*=sa; 
    y*=sa; 
    z*=sa; 

    double qx = q[0]; 
    double qy = q[1]; 
    double qz = q[2]; 
    double qw = q[3]; 

    q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 

} 

вы можете повысить точность, делая половину угла О, которое больше 5 умножений:

final double c3 = 1.0d/(6.0 *4*4*4 ) ; // evaulated in compile time 
    final double c2 = 1.0d/(2.0 *4*4 ) ; // evaulated in compile time 
    double sa_ = 0.25d - c3*r2   ; 
    double ca_ = 1  - c2*r2   ; 
    double ca = ca_*ca_ - sa_*sa_*r2  ; 
    double sa = 2*ca_*sa_     ; 

или даже более точным путем другой угол расщепления до половины:

final double c3 = 1.0d/(6 *8*8*8); // evaulated in compile time 
    final double c2 = 1.0d/(2 *8*8 ); // evaulated in compile time 
    double sa = ( 0.125d - c3*r2)  ; 
    double ca = 1  - c2*r2  ; 
    double ca_ = ca*ca - sa*sa*r2; 
    double sa_ = 2*ca*sa; 
     ca = ca_*ca_ - sa_*sa_*r2; 
     sa = 2*ca_*sa_; 

---

ПРИМЕЧАНИЕ: Если вы используете более сложную схему интеграции, чем просто Верле (например, Рунге-Кутта) вы , вероятно, потребуется дифференциал кватернион, а не новой (обновленной) кватернион.

это можно увидеть в коде здесь

q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 

это может быть истолковано как умножение старого (не обновляется) кватернион по ca (косинус половинного угла), который approximativelly ca ~ 1 для малых углов и добавив остальные (некоторые кросс-взаимодействия). Таким образом, дифференциал просто:

dq[0] = x*qw + y*qz - z*qy + (1-ca)*qx; 
    dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy; 
    dq[2] = x*qy - y*qx + z*qw + (1-ca)*qz; 
    dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw; 

где термин (1 - ca) ~ 0 для малых углов и можно иногда пренебречь (в основном это просто ренормируют quternion).

Answer 3

Простое преобразование из «экспоненциальной карты» в кватернион. (экспоненциальное отображение, равное угловой скорости, умноженное на deltaTime). Результатом кватерниона является дельта-поворот для пройденного дельта-времени и угловой скорости «w».

Vector3 em = w*deltaTime; // exponential map 
{ 
Quaternion q; 
Vector3 ha = em/2.0; // vector of half angle 

double l = ha.norm(); 
if(l > 0) 
{ 
    double ss = sin(l)/l; 
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss); 
}else{ 
    // if l is too small, its norm can be equal 0 but norm_inf greater 0 
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z()); 
}