void Quaternion::FromRotationTo(const Vector3& start, const Vector3& end)
{
Vector3 normStart = start.Normalized();
Vector3 normEnd = end.Normalized();
float d = normStart.DotProduct(normEnd);
if (d > -1.0f + M_EPSILON)
{
Vector3 c = normStart.CrossProduct(normEnd);
float s = sqrtf((1.0f + d) * 2.0f);
float invS = 1.0f / s;
x_ = c.x_ * invS;
y_ = c.y_ * invS;
z_ = c.z_ * invS;
w_ = 0.5f * s;
}
else
{
Vector3 axis = Vector3::RIGHT.CrossProduct(normStart);
if (axis.Length() < M_EPSILON)
axis = Vector3::UP.CrossProduct(normStart);
FromAngleAxis(180.f, axis);
}
}
void Quat::FromDirection(const Vec3& dir){
Real angle = Vec3::UNIT_Y.AngleBetween(dir);
if (IsEqual(angle, 0.f, 0.001f)){
*this = IDENTITY;
}
else{
auto axis = Vec3::UNIT_Y.Cross(dir).NormalizeCopy();
FromAngleAxis(angle, axis);
}
}
void _GetMatrix(Animation* animation, double time, double m[4][4])
{
Animation* a = (Animation*) animation;
/* m = translation * rotation * scale */
/* Rotation. */
if(a->rotations)
{
double drot[4];
_GetRotation(a, time, drot);
Rotation3 RR;
RR.x = drot[0];
RR.y = drot[1];
RR.z = drot[2];
RR.angle = drot[3];
Quaternion q;
FromAngleAxis(&q, RR.angle, RR.x, RR.y, RR.z);
QuatToMatrix(&q, m); /* Also sets bottom row to 0 0 0 1 */
}
else
{
m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}
/* Concatenate with scaling. */
if(a->scales)
{
double S[3];
_GetScale(a, time, S);
m[0][0] *= S[0]; m[0][1] *= S[1]; m[0][2] *= S[2];
m[1][0] *= S[0]; m[1][1] *= S[1]; m[1][2] *= S[2];
m[2][0] *= S[0]; m[2][1] *= S[1]; m[2][2] *= S[2];
}
/* Translation. */
if(a->translations)
{
double P[3];
_GetTranslation(a, time, P);
m[0][3] = P[0];
m[1][3] = P[1];
m[2][3] = P[2];
}
}
void Quaternion::FromRotationTo(const Vector3& start, const Vector3& end)
{
Vector3 normStart = start.Normalized();
Vector3 normEnd = end.Normalized();
float d = normStart.Dot(normEnd);
if (d > -1.0f + Math::kLARGE_EPSILON) {
Vector3 c = normStart.Cross(normEnd);
float s = sqrtf((1.0f + d) * 2.0f);
float invS = 1.0f / s;
x = c.x * invS;
y = c.y * invS;
z = c.z * invS;
w = 0.5f * s;
}
else {
Vector3 axis = Vector3::RIGHT.Cross(normStart);
if (axis.Length() < Math::kLARGE_EPSILON)
axis = Vector3::UP.Cross(normStart);
FromAngleAxis(180.f, axis);
}
}
/*-------------------------------------------------------------------------*/
void* KB_RotInitialize (int numKeys, RotationKey* key)
{
double omt0, omc0, opc0, omb0, opb0, adj0, out0, out1;
double omt1, omc1, opc1, omb1, opb1, adj1, in0, in1;
int i0 = 0, i1 = 1, i2 = 2, i3 = 3;
Quaternion q0, q1, q2, q3;
Quaternion prod;
Quaternion logdq, Tout, Tin, arg0, arg1;
/* assert: numKeys >= 4 */
SplineInfo* info = (SplineInfo*) calloc( 1, sizeof(SplineInfo));
info->numPolys = numKeys-3;
info->si = (SquadInfo*) calloc(info->numPolys, sizeof( SquadInfo ) );
for (/**/; i0 < info->numPolys; i0++, i1++, i2++, i3++)
{
FromAngleAxis(&q0, key[i0].Rot.angle,key[i0].Rot.x,key[i0].Rot.y,
key[i0].Rot.z);
FromAngleAxis(&q1, key[i1].Rot.angle,key[i1].Rot.x,key[i1].Rot.y,
key[i1].Rot.z);
FromAngleAxis(&q2, key[i2].Rot.angle,key[i2].Rot.x,key[i2].Rot.y,
key[i2].Rot.z);
FromAngleAxis(&q3, key[i3].Rot.angle,key[i3].Rot.x,key[i3].Rot.y,
key[i3].Rot.z);
/* arrange for consecutive quaternions to have acute angle */
if ( Dot(&q1, &q2) < 0.0 )
{
/*q2 = -q2;*/
NegSelf(&q2);
ToAngleAxis(&q2, &(key[i2].Rot.angle),&(key[i2].Rot.x),&(key[i2].Rot.y),
&(key[i2].Rot.z));
}
/* build log(q[i1]^{-1}*q[i2]) */
/* prod = q1.UnitInverse()*q2; */
UnitInverse(&prod, &q1); MulSelf(&prod, &q2);
/* logdq = prod.Log(); */
Log(&logdq, &prod);
/* build multipliers at q[i1] */
omt0 = 1-key[i1].tension;
omc0 = 1-key[i1].continuity;
opc0 = 1+key[i1].continuity;
omb0 = 1-key[i1].bias;
opb0 = 1+key[i1].bias;
adj0 = 2*(key[i2].t-key[i1].t)/(key[i2].t-key[i0].t);
out0 = 0.5*adj0*omt0*opc0*opb0;
out1 = 0.5*adj0*omt0*omc0*omb0;
/* build outgoing tangent at q[i1] */
/* prod = q0.UnitInverse()*q1; */
UnitInverse(&prod, &q0); MulSelf(&prod, &q1);
/* Quaternion Tout = out1*logdq+out0*prod.Log(); */
MulScal(&Tout, &logdq, out1); LogSelf(&prod); MulScal(&prod, &prod, out0);
AddSelf(&Tout, &prod);
/* build multipliers at q[i2] */
omt1 = 1-key[i2].tension;
omc1 = 1-key[i2].continuity;
opc1 = 1+key[i2].continuity;
omb1 = 1-key[i2].bias;
opb1 = 1+key[i2].bias;
adj1 = 2*(key[i2].t-key[i1].t)/(key[i3].t-key[i1].t);
in0 = 0.5*adj1*omt1*omc1*opb1;
in1 = 0.5*adj1*omt1*opc1*omb1;
/* build incoming tangent at q[i2] */
/* prod = q2.UnitInverse()*q3; */
UnitInverse(&prod, &q2); MulSelf(&prod, &q3);
/* Quaternion Tin = in1*prod.Log()+in0*logdq; */
MulScal(&Tin, &logdq, in0); LogSelf(&prod); MulScal(&prod, &prod, in1);
AddSelf(&Tin, &prod);
/*info->si[i0].p = q1;*/
SetQuat(&(info->si[i0].p), &q1);
/*info->si[i0].q = q2; */
SetQuat(&(info->si[i0].q), &q2);
/* Quaternion arg0 = 0.5*(Tout-logdq); */
Sub(&arg0, &Tout, &logdq); MulScalSelf(&arg0, 0.5);
/* Quaternion arg1 = 0.5*(logdq-Tin); */
Sub(&arg1, &logdq, &Tin); MulScalSelf(&arg1, 0.5);
/* info->si[i0].a = q1*arg0.Exp(); */
/* info->si[i0].b = q2*arg1.Exp(); */
ExpSelf(&arg0); Mul(&(info->si[i0].a), &q1, &arg0);
ExpSelf(&arg1); Mul(&(info->si[i0].b), &q2, &arg1);
info->si[i0].tmin = key[i1].t;
info->si[i0].tmax = key[i2].t;
info->si[i0].trange = info->si[i0].tmax-info->si[i0].tmin;
}
return info;
}
void FromAngleAxisPt(Quaternion* q, float rdAngle, Point3* rkPoint)
{
FromAngleAxis(q, rdAngle, rkPoint->x, rkPoint->y, rkPoint->z);
}
cQuaternion::cQuaternion(float afAngle, const cVector3f & avAxis)
{
FromAngleAxis(afAngle,avAxis);
}
void CQuater::FromAngleAxis(float fAngle, const CVertex& v) {
FromAngleAxis(fAngle, v.x, v.y, v.z);
}
Quat::Quat(Real radian, const Vec3& axis)
{
FromAngleAxis(radian, axis);
}