/*---------------------------------------------------------------------*//**
積
**//*---------------------------------------------------------------------*/
Vector4F Quaternion::multiply(const Vector4F& q1, const Vector4F& q2)
{
Vector4F ret;
ret.x() = q1.y() * q2.z() - q1.z() * q2.y() + q1.w() * q2.x() + q1.x() * q2.w();
ret.y() = q1.z() * q2.x() - q1.x() * q2.z() + q1.w() * q2.y() + q1.y() * q2.w();
ret.z() = q1.x() * q2.y() - q1.y() * q2.x() + q1.w() * q2.z() + q1.z() * q2.w();
ret.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
return ret;
}
/*---------------------------------------------------------------------*//**
log
**//*---------------------------------------------------------------------*/
Vector4F Quaternion::log(const Vector4F& q)
{
f32 a = acosf(q.w());
f32 sina = sinf(a);
Vector4F ret;
if(sina > 0.0f)
{
f32 sinai = 1.0f / sina;
ret.x() = a * q.x() * sinai;
ret.y() = a * q.y() * sinai;
ret.z() = a * q.z() * sinai;
}
return ret;
}
/*---------------------------------------------------------------------*//**
Exponential
**//*---------------------------------------------------------------------*/
Vector4F Quaternion::exp(const Vector4F& q)
{
f32 a = sqrtf(q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
f32 sina = sinf(a);
f32 cosa = cosf(a);
Vector4F ret;
ret.w() = cosa;
if(a > 0.0f)
{
f32 ai = 1.0f / a;
ret.x() = sina * q.x() * ai;
ret.y() = sina * q.y() * ai;
ret.z() = sina * q.z() * ai;
}
return ret;
}
/*---------------------------------------------------------------------*//**
スプライン補間
**//*---------------------------------------------------------------------*/
void Quaternion::spline(Vector4F* ret, const Vector4F* qnm1, const Vector4F* qn, const Vector4F* qnp1)
{
Vector4F qni;
qni.x() = - qn->x();
qni.y() = - qn->y();
qni.z() = - qn->z();
qni.w() = qn->w();
*ret = multiply(*qn, exp((log(multiply(qni, *qnm1)) + log(multiply(qni, *qnp1))) * -0.25f));
}
Vector4F operator - (float scalar, const Vector4F& rVector)
{
scalar = scalar * rVector.w();
return Vector4F(rVector.x() - scalar, rVector.y() - scalar, rVector.z() - scalar, rVector.w());
}
Vector4F operator - (const Vector4F& rVector)
{
return Vector4F(-rVector.x(), -rVector.y(), -rVector.z(), rVector.w());
}
