Youssef Assem
c2882035b5
animation , animator , keyframe & quaterions compiling but not debuged at all
105 lines
2.7 KiB
C++
105 lines
2.7 KiB
C++
#include "quaternion.h"
|
|
#include <math.h>
|
|
|
|
Quaternion::Quaternion(float x, float y, float z, float w)
|
|
{
|
|
x_ = x;
|
|
y_ = y;
|
|
z_ = z;
|
|
w_ = w;
|
|
normalize();
|
|
}
|
|
|
|
void Quaternion::normalize()
|
|
{
|
|
float mag = (float)sqrt(w_ * w_ + x_ * x_ + y_ * y_ + z_ * z_);
|
|
w_ /= mag;
|
|
x_ /= mag;
|
|
y_ /= mag;
|
|
z_ /= mag;
|
|
}
|
|
|
|
Matrix Quaternion::toRotationMatrix()
|
|
{
|
|
Matrix matrix;
|
|
float xy = x_ * y_;
|
|
float xz = x_ * z_;
|
|
float xw = x_ * w_;
|
|
float yz = y_ * z_;
|
|
float yw = y_ * w_;
|
|
float zw = z_ * w_;
|
|
float xSquared = x_ * x_;
|
|
float ySquared = y_ * y_;
|
|
float zSquared = z_ * z_;
|
|
|
|
Vec4f c0 = Vec4f(1 - 2 * (ySquared + zSquared), 2 * (xy - zw), 2 * (xz + yw),0);
|
|
Vec4f c1 = Vec4f(2 * (xy + zw), 1 - 2 * (xSquared + zSquared), 2 * (yz - xw), 0);
|
|
Vec4f c2 = Vec4f(2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xSquared + ySquared), 0);
|
|
Vec4f c3 = Vec4f(0,0,0,1);
|
|
|
|
matrix.set_col(0,c0);
|
|
matrix.set_col(1, c1);
|
|
matrix.set_col(2, c2);
|
|
matrix.set_col(3, c3);
|
|
return matrix;
|
|
}
|
|
|
|
Quaternion Quaternion::fromMatrix(Matrix matrix)
|
|
{
|
|
float w_, x_, y_, z_;
|
|
float diagonal = matrix[0][0] + matrix[1][1] + matrix[2][2];
|
|
if (diagonal > 0) {
|
|
float w4 = (float)(sqrt(diagonal + 1.f) * 2.f);
|
|
w_ = w4 / 4.f;
|
|
x_ = (matrix[2][1] - matrix[1][2]) / w4;
|
|
y_ = (matrix[0][2] - matrix[2][0]) / w4;
|
|
z_ = (matrix[1][0] - matrix[0][1]) / w4;
|
|
}
|
|
else if ((matrix[0][0] > matrix[1][1]) && (matrix[0][0] > matrix[2][2])) {
|
|
float x4 = (float)(sqrt(1.f + matrix[0][0] - matrix[1][1] - matrix[2][2]) * 2.f);
|
|
w_ = (matrix[2][1] - matrix[1][2]) / x4;
|
|
x_ = x4 / 4.f;
|
|
y_ = (matrix[0][1] + matrix[1][0]) / x4;
|
|
z_ = (matrix[0][2] + matrix[2][0]) / x4;
|
|
}
|
|
else if (matrix[1][1] > matrix[2][2]) {
|
|
float y4 = (float)(sqrt(1.f + matrix[1][1] - matrix[0][0] - matrix[2][2]) * 2.f);
|
|
w_ = (matrix[0][2] - matrix[2][0]) / y4;
|
|
x_ = (matrix[0][1] + matrix[1][0]) / y4;
|
|
y_ = y4 / 4.f;
|
|
z_ = (matrix[1][2] + matrix[2][1]) / y4;
|
|
}
|
|
else {
|
|
float z4 = (float)(sqrt(1.f + matrix[2][2] - matrix[0][0] - matrix[1][1]) * 2.f);
|
|
w_ = (matrix[1][0] - matrix[0][1]) / z4;
|
|
x_ = (matrix[0][2] + matrix[2][0]) / z4;
|
|
y_ = (matrix[1][2] + matrix[2][1]) / z4;
|
|
z_ = z4 / 4.f;
|
|
}
|
|
return Quaternion(x_, y_, z_, w_);
|
|
}
|
|
|
|
Quaternion Quaternion::interpolate(Quaternion a, Quaternion b, float blend)
|
|
{
|
|
Quaternion result(0, 0, 0, 1);
|
|
float dot = a.w_ * b.w_ + a.x_ * b.x_ + a.y_ * b.y_ + a.z_ * b.z_;
|
|
float blendI = 1.f - blend;
|
|
if (dot < 0) {
|
|
result.w_ = blendI * a.w_ + blend * -b.w_;
|
|
result.x_ = blendI * a.x_ + blend * -b.x_;
|
|
result.y_ = blendI * a.y_ + blend * -b.y_;
|
|
result.z_ = blendI * a.z_ + blend * -b.z_;
|
|
}
|
|
else {
|
|
result.w_ = blendI * a.w_ + blend * b.w_;
|
|
result.x_ = blendI * a.x_ + blend * b.x_;
|
|
result.y_ = blendI * a.y_ + blend * b.y_;
|
|
result.z_ = blendI * a.z_ + blend * b.z_;
|
|
}
|
|
result.normalize();
|
|
return result;
|
|
}
|
|
|
|
|
|
|