void RVToQuat(const Vec3f &_v,Quat &q)
{
q.v=_v;
q.w=_v.GetNorm();
q.v.CNormalize();
q.w*=0.5f;
q.xyz()*=Sin(q.w); //Don't try to use sinf here...we already tried, it breaks the quaternion...Need to investigate
q.w=Cos(q.w); //Don't try to use cosf here...we already tried, it breaks the quaternion...Need to investigate
}
Quat Util_L::GetOrientationQuat(const Vec3f &_nodePos, const Vec3f &_viewPos, const Vec3f &_baseUp, S32 _order[3], Float *_pDist)
{
Vec3f vFrontDir = _viewPos - _nodePos;
if (_pDist)
{
(*_pDist) = vFrontDir.GetNorm();
}
vFrontDir.CNormalize();
return GetOrientationQuat(vFrontDir, _baseUp, _order);
}
Quat::Quat(const Vec3f &V1,const Vec3f &V2)
{
Quat q;
// normer les 2 vecteurs et sortir si un vecteur est trop petit et qu'on a pas r�ussi � normer
Float n1=V1.GetNorm();
Float n2=V2.GetNorm();
if ((n1<Float_Eps) || (n2<Float_Eps))
{
// Error Quat !!!
xyzw().SetDefault();
Normalize();
return;
}
Vec3f v1=V1* (1.f/n1);
Vec3f v2=V2* (1.f/n2);
if (v1*v2 >= 0.f)
{
// Angle < 90
Vec3f v3=v1+v2;
v3.Normalize();
v = v1^v3;
Float Sin2 = v*v;
//Should be Float_Eps*Float_Eps?
if (Sin2 < Float_Eps)
{
// Pas de rotation
xyzw().SetDefault();
Normalize();
return;
}
Float tempf=1.f-Sin2;
if (tempf<0.f) tempf=0.f;
if (tempf>1.f) tempf=1.f;
w = Sqrt(tempf);
Normalize();
return;
}
// Angle > 90
// On travaille avec le vecteur oppos�.
Vec3f v3=v2-v1;
v3.CNormalize();
v = v1^v3;
Float Sin2 = v*v;
if (Sin2 < Float_Eps)
{
// Rotation 180.
Float x = Abs(v1.x);
Float y = Abs(v1.y);
Float z = Abs(v1.z);
if ((v1.y > -0.707106f) && (v1.y < 0.707106f))
// if ((y < x) && (y < z))
{
v3.x = v1.z;
v3.y = v1.y;
v3.z = v1.x;
}
else
{
v3.x = v1.z;
v3.y = v1.x;
v3.z = v1.y;
}
v = v1^v3;
v.CNormalize();
w=0.f;
Normalize();
return;
}
w = Sqrt(Sin2);
if (w)
v *= Sqrt(1.f-Sin2) / w;
Normalize();
}
