d3dx9_36: Implementation of D3DXQuaternionSquadSetup.

dlls/d3dx9_36/d3dx9_36.spec

@@ -252,7 +252,7 @@
 @ stdcall D3DXQuaternionRotationYawPitchRoll(ptr float float float)
 @ stdcall D3DXQuaternionSlerp(ptr ptr ptr float)
 @ stdcall D3DXQuaternionSquad(ptr ptr ptr ptr ptr float)
-@ stub D3DXQuaternionSquadSetup(ptr ptr ptr ptr ptr ptr ptr)
+@ stdcall D3DXQuaternionSquadSetup(ptr ptr ptr ptr ptr ptr ptr)
 @ stdcall D3DXQuaternionToAxisAngle(ptr ptr ptr)
 @ stub D3DXRectPatchSize(ptr ptr ptr)
 @ stub D3DXSaveMeshHierarchyToFileA(ptr long ptr ptr ptr)

dlls/d3dx9_36/math.c

@@ -1366,6 +1366,73 @@ D3DXQUATERNION* WINAPI D3DXQuaternionSquad(D3DXQUATERNION *pout, CONST D3DXQUATE
     return pout;
 }
 
+static D3DXQUATERNION add_diff(CONST D3DXQUATERNION *q1, CONST D3DXQUATERNION *q2, CONST FLOAT add)
+{
+    D3DXQUATERNION temp;
+
+    temp.x = q1->x + add * q2->x;
+    temp.y = q1->y + add * q2->y;
+    temp.z = q1->z + add * q2->z;
+    temp.w = q1->w + add * q2->w;
+
+    return temp;
+}
+
+void WINAPI D3DXQuaternionSquadSetup(D3DXQUATERNION *paout, D3DXQUATERNION *pbout, D3DXQUATERNION *pcout, CONST D3DXQUATERNION *pq0, CONST D3DXQUATERNION *pq1, CONST D3DXQUATERNION *pq2, CONST D3DXQUATERNION *pq3)
+{
+    D3DXQUATERNION q, temp1, temp2, temp3, zero;
+
+    TRACE("(%p, %p, %p, %p, %p, %p, %p)\n", paout, pbout, pcout, pq0, pq1, pq2, pq3);
+
+    zero.x = 0.0f;
+    zero.y = 0.0f;
+    zero.z = 0.0f;
+    zero.w = 0.0f;
+
+    if ( D3DXQuaternionDot(pq0, pq1) <  0.0f )
+        temp2 = add_diff(&zero, pq0, -1.0f);
+    else
+        temp2 = *pq0;
+
+    if ( D3DXQuaternionDot(pq1, pq2) < 0.0f )
+        *pcout = add_diff(&zero, pq2, -1.0f);
+    else
+        *pcout = *pq2;
+
+    if ( D3DXQuaternionDot(pcout, pq3) < 0.0f )
+        temp3 = add_diff(&zero, pq3, -1.0f);
+    else
+        temp3 = *pq3;
+
+    D3DXQuaternionInverse(&temp1, pq1);
+    D3DXQuaternionMultiply(&temp2, &temp1, &temp2);
+    D3DXQuaternionLn(&temp2, &temp2);
+    D3DXQuaternionMultiply(&q, &temp1, pcout);
+    D3DXQuaternionLn(&q, &q);
+    temp1 = add_diff(&temp2, &q, 1.0f);
+    temp1.x *= -0.25f;
+    temp1.y *= -0.25f;
+    temp1.z *= -0.25f;
+    temp1.w *= -0.25f;
+    D3DXQuaternionExp(&temp1, &temp1);
+    D3DXQuaternionMultiply(paout, pq1, &temp1);
+
+    D3DXQuaternionInverse(&temp1, pcout);
+    D3DXQuaternionMultiply(&temp2, &temp1, pq1);
+    D3DXQuaternionLn(&temp2, &temp2);
+    D3DXQuaternionMultiply(&q, &temp1, &temp3);
+    D3DXQuaternionLn(&q, &q);
+    temp1 = add_diff(&temp2, &q, 1.0f);
+    temp1.x *= -0.25f;
+    temp1.y *= -0.25f;
+    temp1.z *= -0.25f;
+    temp1.w *= -0.25f;
+    D3DXQuaternionExp(&temp1, &temp1);
+    D3DXQuaternionMultiply(pbout, pcout, &temp1);
+
+    return;
+}
+
 void WINAPI D3DXQuaternionToAxisAngle(CONST D3DXQUATERNION *pq, D3DXVECTOR3 *paxis, FLOAT *pangle)
 {
     paxis->x = pq->x;

dlls/d3dx9_36/tests/math.c

@@ -940,6 +940,48 @@ static void D3DXQuaternionTest(void)
     D3DXQuaternionSquad(&gotquat,&q,&r,&t,&u,scale);
     expect_vec4(expectedquat,gotquat);
 
+/*_______________D3DXQuaternionSquadSetup___________________*/
+    r.x = 1.0f, r.y = 2.0f; r.z = 4.0f; r.w = 10.0f;
+    s.x = -3.0f; s.y = 4.0f; s.z = -5.0f; s.w = 7.0;
+    t.x = -1111.0f, t.y = 111.0f; t.z = -11.0f; t.w = 1.0f;
+    u.x = 91.0f; u.y = - 82.0f; u.z = 7.3f; u.w = -6.4f;
+    D3DXQuaternionSquadSetup(&gotquat,&Nq,&Nq1,&r,&s,&t,&u);
+    expectedquat.x = 7.121285f; expectedquat.y = 2.159964f; expectedquat.z = -3.855094f; expectedquat.w = 5.362844f;
+    expect_vec4(expectedquat,gotquat);
+    expectedquat.x = -1113.492920f; expectedquat.y = 82.679260f; expectedquat.z = -6.696645f; expectedquat.w = -4.090050f;
+    expect_vec4(expectedquat,Nq);
+    expectedquat.x = -1111.0f; expectedquat.y = 111.0f; expectedquat.z = -11.0f; expectedquat.w = 1.0f;
+    expect_vec4(expectedquat,Nq1);
+    r.x = 0.2f; r.y = 0.3f; r.z = 1.3f; r.w = -0.6f;
+    s.x = -3.0f; s.y =-2.0f; s.z = 4.0f; s.w = 0.2f;
+    t.x = 0.4f; t.y = 8.3f; t.z = -3.1f; t.w = -2.7f;
+    u.x = 1.1f; u.y = -0.7f; u.z = 9.2f; u.w = 0.0f;
+    D3DXQuaternionSquadSetup(&gotquat,&Nq,&Nq1,&r,&s,&u,&t);
+    expectedquat.x = -4.139569f; expectedquat.y = -2.469115f; expectedquat.z = 2.364477f; expectedquat.w = 0.465494f;
+    expect_vec4(expectedquat,gotquat);
+    expectedquat.x = 2.342533f; expectedquat.y = 2.365127f; expectedquat.z = 8.628538f; expectedquat.w = -0.898356f;
+    expect_vec4(expectedquat,Nq);
+    expectedquat.x = 1.1f; expectedquat.y = -0.7f; expectedquat.z = 9.2f; expectedquat.w = 0.0f;
+    expect_vec4(expectedquat,Nq1);
+    D3DXQuaternionSquadSetup(&gotquat,&Nq,&Nq1,&r,&s,&t,&u);
+    expectedquat.x = -3.754567f; expectedquat.y = -0.586085f; expectedquat.z = 3.815818f; expectedquat.w = -0.198150f;
+    expect_vec4(expectedquat,gotquat);
+    expectedquat.x = 0.140773f; expectedquat.y = -8.737090f; expectedquat.z = -0.516593f; expectedquat.w = 3.053942f;
+    expect_vec4(expectedquat,Nq);
+    expectedquat.x = -0.4f; expectedquat.y = -8.3f; expectedquat.z = 3.1f; expectedquat.w = 2.7f;
+    expect_vec4(expectedquat,Nq1);
+    r.x = -1.0f; r.y = 0.0f; r.z = 0.0f; r.w = 0.0f;
+    s.x = 1.0f; s.y =0.0f; s.z = 0.0f; s.w = 0.0f;
+    t.x = 1.0f; t.y = 0.0f; t.z = 0.0f; t.w = 0.0f;
+    u.x = -1.0f; u.y = 0.0f; u.z = 0.0f; u.w = 0.0f;
+    D3DXQuaternionSquadSetup(&gotquat,&Nq,&Nq1,&r,&s,&t,&u);
+    expectedquat.x = 1.0f; expectedquat.y = 0.0f; expectedquat.z = 0.0f; expectedquat.w = 0.0f;
+    expect_vec4(expectedquat,gotquat);
+    expectedquat.x = 1.0f; expectedquat.y = 0.0f; expectedquat.z = 0.0f; expectedquat.w = 0.0f;
+    expect_vec4(expectedquat,Nq);
+    expectedquat.x = 1.0f; expectedquat.y = 0.0f; expectedquat.z = 0.0f; expectedquat.w = 0.0f;
+    expect_vec4(expectedquat,Nq1);
+
 /*_______________D3DXQuaternionToAxisAngle__________________*/
     Nq.x = 1.0f/22.0f; Nq.y = 2.0f/22.0f; Nq.z = 4.0f/22.0f; Nq.w = 10.0f/22.0f;
     expectedvec.x = 1.0f/22.0f; expectedvec.y = 2.0f/22.0f; expectedvec.z = 4.0f/22.0f;

include/d3dx9math.h

@@ -337,6 +337,7 @@ D3DXQUATERNION* WINAPI D3DXQuaternionRotationMatrix(D3DXQUATERNION *pout, CONST
 D3DXQUATERNION* WINAPI D3DXQuaternionRotationYawPitchRoll(D3DXQUATERNION *pout, FLOAT yaw, FLOAT pitch, FLOAT roll);
 D3DXQUATERNION* WINAPI D3DXQuaternionSlerp(D3DXQUATERNION *pout, CONST D3DXQUATERNION *pq1, CONST D3DXQUATERNION *pq2, FLOAT t);
 D3DXQUATERNION* WINAPI D3DXQuaternionSquad(D3DXQUATERNION *pout, CONST D3DXQUATERNION *pq1, CONST D3DXQUATERNION *pq2, CONST D3DXQUATERNION *pq3, CONST D3DXQUATERNION *pq4, FLOAT t);
+void WINAPI D3DXQuaternionSquadSetup(D3DXQUATERNION *paout, D3DXQUATERNION *pbout, D3DXQUATERNION *pcout, CONST D3DXQUATERNION *pq0, CONST D3DXQUATERNION *pq1, CONST D3DXQUATERNION *pq2, CONST D3DXQUATERNION *pq3);
 void WINAPI D3DXQuaternionToAxisAngle(CONST D3DXQUATERNION *pq, D3DXVECTOR3 *paxis, FLOAT *pangle);
 
 D3DXVECTOR2* WINAPI D3DXVec2BaryCentric(D3DXVECTOR2 *pout, CONST D3DXVECTOR2 *pv1, CONST D3DXVECTOR2 *pv2, CONST D3DXVECTOR2 *pv3, FLOAT f, FLOAT g);