더 나은 방법을 찾고 Quaternion 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 

을 그래서 지금은 Q에 저장 차동 사원 수 있고 그때 단순히이 차등 쿼터니언 추가하여 사원 수를 업데이트합니다.

이 방법은 강체의 움직임을 예측하는 데 적합합니까? 또는 각속도의 쿼터니언을 예측하는 더 좋은 방법이 있습니까? 이 작동하지만 예상되는 결과를 얻지 못하고 있습니다.

Answer 1

몇 가지 일이있을 수 있습니다. 당신은 그 쿼터니온을 다시 정규화하는 것에 대해서는 언급하지 않았습니다. 네가 그렇게하지 않으면 나쁜 일은 분명히 일어날 것이다. 델타 - 쿼터니온 구성 요소를 원래의 쿼터니언에 추가하기 전에 dt이 경과 한 시간만큼 곱하면된다는 말도 아닙니다. 각속도가 초당 라디안인데 단 몇 초 만에 앞으로 나아갈 경우 지나치게 멀리 나아갑니다. 그러나, 그렇다고하더라도, 당신은 이산적인 양의 시간을 밟아서 극소수 인 것처럼하려고 시도하기 때문에, 특히 당신의 타임 스텝이나 각속도가 큰 경우, 이상한 일이 일어날 것입니다.

물리 엔진 ODE는 몸체의 회전을 각속도에서 미소 한 단계를 취하는 것처럼 업데이트하거나 유한 크기의 단계를 사용하여 업데이트하는 옵션을 제공합니다. 유한 단계는 훨씬 정확하지만 일부 삼각 함수가 필요합니다. 함수와 너무 조금 느립니다. 관련 ODE 소스 코드는 here, lines 300-321이며 델타 - 쿼터니언 코드는 here, line 310입니다. 이 분할 0으로 문제를 방지 할 수 있으며 여전히 매우 정확

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

: sinc(x)가

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; 

.

쿼터니언 q은 신체의 방향의 기존 쿼터니언 표현에 미리 곱해진다. 그런 다음 다시 정규화하십시오. -

---

편집이 공식에서 오는 경우 :

초기 사원 수 q0 시간의 dt 금액에 대한 각속도 w으로 회전 한 후 결과 최종 쿼터니언 q1을 고려하십시오. 여기서 우리가하는 것은 각속도 벡터를 쿼터니언으로 바꾸고 그 쿼터니온에 의해 첫 번째 방향을 회전시키는 것입니다. 쿼터니온과 각속도는 축각 표현의 변형입니다. 인 몸체는 단위 축 주위로 theta에 의해 정준 방향에서 으로 회전되고 방향은 다음과 같은 쿼터니언 표현을 갖습니다. q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]. 단위 축 [x,y,z] 주위에 인 몸체theta/s은 각속도가 w=[theta*x theta*y theta*z]입니다. 따라서 dt 초 동안 회전이 얼마나 발생할지 결정하려면 먼저 각속도 크기 인 theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2)을 추출합니다. 그런 다음 우리는 dt을 곱하여 실제 각도를 찾습니다 (그리고 이것을 4 원으로 바꾸는 데 편의를 위해 동시에 2로 나눕니다).[x y z] 축을 정규화해야하므로 theta으로 나눕니다. 그것이 sinc(theta) 부분의 출처입니다. (theta에는 크기가 작기 때문에 0.5*dt이 추가로 있으므로이 값을 배수합니다.) sinc(x) 함수는 숫자가 안정되고 정확하기 때문에 x이 작을 때 함수의 테일러 급수 근사법을 사용하고 있습니다. 이 편리한 기능을 사용할 수있는 능력은 실제 크기 인 wMag으로 나누지 않은 이유입니다. 매우 빠르게 회전하지 않는 물체는 매우 작은 각속도를 갖습니다. 우리가 이것을 매우 일반적으로 기대하기 때문에 안정적인 솔루션이 필요합니다. 우리가 끝내는 것은 회전의 단일 단계 시간 단계 dt을 나타내는 쿼터니언입니다. 벡터는 속도와 방법에 증가 회전 상태를 나타내는 quaterniom (즉, 회전 운동의 미분 방정식을 통합) 각 dphi (작은 벡터 증가 정확도 사이에 아주 좋은 트레이드 오프와 metod이 있습니다

Answer 2

각속도 omega 시간 단계별로 작성 dt).

벡터로 사원 수의 회전 정확한 (느린) 방법 :

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처럼 느린 함수를 계산해야한다는 것입니다. 대신에 norm 대신에 norm^2을 단지 사용하여 표현 된 테일러 확장에 의해 sin 및 cos을 근사하여 작은 각에 대해 상당한 속도 증가 및 합리적인 정확도를 얻을 수 있습니다 (시뮬레이션의 시간 단계가 적당 할 경우). 벡터로 사원 수의 회전이 빠른 방법처럼

: 당신은 절반 오 각도를 수행하여 정확도를 높일 수 있습니다

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_; 

012 317,894,922,398,581,088,243,210

참고 : 그냥 verlet보다 더 정교한 통합 방식을 사용하는 경우 (같은 룽게 - 쿠타)를 오히려 새 (업데이트) 쿼터니언보다, 사원 수의 차이를 필요 아마 것이다.

이이 작은 각도에 대해 approximativelly ca ~ 1이다 ca (반 화각의 코사인 변환)에 의해 구 (업데이트 안됨) 사원 수의 승산으로 해석 될 수있는 여기

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; 

코드에 표시 할 수있다 나머지 부분을 추가합니다 (일부 상호 작용). 그래서 단순히 차 :

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을 곱한 것과 같은 지수지도). 결과 quaternion은 전달 된 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()); 
}