/* Interpolation between two quaternions */
LPD3DRMQUATERNION WINAPI D3DRMQuaternionSlerp(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b, D3DVALUE alpha)
{
D3DVALUE dot, epsilon, temp, theta, u;
D3DVECTOR v1, v2;
dot = a->s * b->s + D3DRMVectorDotProduct(&a->v, &b->v);
epsilon = 1.0f;
temp = 1.0f - alpha;
u = alpha;
if (dot < 0.0)
{
epsilon = -1.0;
dot = -dot;
}
if( 1.0f - dot > 0.001f )
{
theta = acos(dot);
temp = sin(theta * temp) / sin(theta);
u = sin(theta * alpha) / sin(theta);
}
q->s = temp * a->s + epsilon * u * b->s;
D3DRMVectorScale(&v1, &a->v, temp);
D3DRMVectorScale(&v2, &b->v, epsilon * u);
D3DRMVectorAdd(&q->v, &v1, &v2);
return q;
}
STDMETHODIMP CVectorObject::RotateAboutAxis(float fTheta, float fAxisX, float fAxisY, float fAxisZ)
{
D3DVECTOR rlvThis, rlvRotated, rlvAxis;
D3DVALUE valModulus;
HRESULT hr = S_OK;
if (fAxisX == 0.0f && fAxisY == 0.0f && fAxisZ == 0.0f)
return E_INVALIDARG;
rlvThis.x = m_x;
rlvThis.y = m_y;
rlvThis.z = m_z;
rlvAxis.x = fAxisX;
rlvAxis.y = fAxisY;
rlvAxis.z = fAxisZ;
valModulus = D3DRMVectorModulus(&rlvThis);
D3DRMVectorRotate(&rlvRotated, &rlvThis, &rlvAxis, fTheta);
// D3DRMVectorRotate will return a unit vector. We need the original size vector.
D3DRMVectorScale(&rlvRotated, &rlvRotated, valModulus);
hr = set(rlvRotated.x, rlvRotated.y, rlvRotated.z);
return hr;
}
STDMETHODIMP CVectorObject::Rotate(float fTheta, IVector *pvres)
{
D3DVECTOR rlvThis, rlvRotated, rlvAxis;
D3DVALUE valModulus;
if (!pvres)
return E_POINTER;
rlvThis.x = m_x;
rlvThis.y = m_y;
rlvThis.z = m_z;
rlvAxis.x = 0.0F;
rlvAxis.y = 1.0F;
rlvAxis.z = 0.0F;
valModulus = D3DRMVectorModulus(&rlvThis);
D3DRMVectorRotate(&rlvRotated, &rlvThis, &rlvAxis, fTheta);
// D3DRMVectorRotate will return a unit vector. We need the original size vector.
D3DRMVectorScale(&rlvRotated, &rlvRotated, valModulus);
pvres->set(rlvRotated.x, rlvRotated.y, rlvRotated.z);
return S_OK;
}
/* Reflection of a vector on a surface */
LPD3DVECTOR WINAPI D3DRMVectorReflect(LPD3DVECTOR r, LPD3DVECTOR ray, LPD3DVECTOR norm)
{
D3DVECTOR sca, temp;
D3DRMVectorSubtract(&temp, D3DRMVectorScale(&sca, norm, 2.0*D3DRMVectorDotProduct(ray,norm)), ray);
*r = temp;
return r;
}
/* Rotation of a vector */
LPD3DVECTOR WINAPI D3DRMVectorRotate(LPD3DVECTOR r, LPD3DVECTOR v, LPD3DVECTOR axis, D3DVALUE theta)
{
D3DRMQUATERNION quaternion1, quaternion2, quaternion3;
D3DVECTOR norm;
quaternion1.s = cos(theta * 0.5f);
quaternion2.s = cos(theta * 0.5f);
norm = *D3DRMVectorNormalize(axis);
D3DRMVectorScale(&quaternion1.v, &norm, sin(theta * 0.5f));
D3DRMVectorScale(&quaternion2.v, &norm, -sin(theta * 0.5f));
quaternion3.s = 0.0;
quaternion3.v = *v;
D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion3);
D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion2);
*r = *D3DRMVectorNormalize(&quaternion1.v);
return r;
}
/* Normalize a vector. Returns (1,0,0) if INPUT is the NULL vector. */
LPD3DVECTOR WINAPI D3DRMVectorNormalize(LPD3DVECTOR u)
{
D3DVALUE modulus = D3DRMVectorModulus(u);
if(modulus)
{
D3DRMVectorScale(u,u,1.0/modulus);
}
else
{
u->u1.x=1.0;
u->u2.y=0.0;
u->u3.z=0.0;
}
return u;
}
/* Return a unit quaternion that represents a rotation of an angle around an axis */
LPD3DRMQUATERNION WINAPI D3DRMQuaternionFromRotation(LPD3DRMQUATERNION q, LPD3DVECTOR v, D3DVALUE theta)
{
q->s = cos(theta/2.0);
D3DRMVectorScale(&q->v, D3DRMVectorNormalize(v), sin(theta/2.0));
return q;
}