Кватернионы и вращение векторов с их помощью. Часть 2.

JazzyJohn опубликовал в Программирование 23 сентября 2013, 16:12

Кватернионы и углы Эйлера.

Не смотря на то, что кватернионы являются намного более мощным инструментом, для вращения, чем углы Эйлера, им не хватает наглядности. Поэтому если вам необходимо задать какое-то простое вращение, например стартовое, то намного нагляднее сделать это с помощью углов Эйлера, чем с помощью кватернионов. Следовательно, надо найти биекцию между углами Эйлера и нормализованными кватернионами.
Для начала вспомним что p = [sin(α/2)u,cos(α/2)w], где u мнимая часть кватерниона и соответствующая оси вращения, а w действительная часть кватерниона соответствующая углу поворота.
Отсюда легко получить повороты вокруг основных осей углов эйлера

px = [cos (y/2), (sin(y/2), 0, 0)] (этот кватернион соответствует повороту roll)
py = [cos (q/2), (0, sin(q/2), 0)] (этот – pitch)
pz = [cos (f/2), (0, 0, sin(f/2))] (этот –yaw)
p = pz py px

С помощью этой формулы можно легко получить следующую таблицу:

w	x	y	z	Описание
1	0	0	0	Определение кватерниона, без вращения
0	1	0	0	Вращение на 180° по оси Х
0	0	1	0	Вращение на 180° по оси Y
0	0	0	1	Вращение на 180° по оси Z
sqrt(0.5)	sqrt(0.5)	0	0	Вращение на 90° по оси Х
sqrt(0.5)	0	sqrt(0.5)	0	Вращение на 90° по оси Y
sqrt(0.5)	0	0	sqrt(0.5)	Вращение на 90° по оси Z
sqrt(0.5)	-sqrt(0.5)	0	0	Вращение на -90° по оси Х
sqrt(0.5)	0	-sqrt(0.5)	0	Вращение на -90° по оси Y
sqrt(0.5)	0	0	-sqrt(0.5)	Вращение на -90° по оси Z

Теперь определим, какую структуру мы будем считать кватернионом в дальнейших листингах:


Struct Quaternion{
	float x;
    	float y;
	float z;
              float w;

};
typedef struct Quaternion Quaternion;

Теперь запишем пример реализации нашей биекции для получения кватерниона из углов Эйлера


	Quaternion QuaternionFromEuler (Vector axis, float angle) {
  		Quaternion result;
  		float sinAngle;
  
  		angle *= 0.5f;
  		VectorNormalize(&axis);
  		sinAngle = sin(angle);
  		result.x = (axis.x * sinAngle);
  		result.y = (axis.y * sinAngle);
  		result.z = (axis.z * sinAngle);
  		result.w = cos(angle);
  		return result;
	}

Чуть ниже будет реализована функция перемножения для кватернионов, которая позволит целиком получить реализацию нашего отображения, а не только вокруг конкретной оси.

Кватернионы и матрица поворота.

Теперь чтобы окончательно удостоверится, что мы можем работать исключительно с кватернионами, не смотря на API, в котором мы работам, нам понадобиться получить связь между кватернионами и матрицами поворота. Будем считать, что матрица трансформации у нас 4х4 при чем в дополнительные строчку и столбец выставлены 0, за исключением пересечения там стоит 1.

Для начала получим матрицу поворота из существующего кватерниона.
Воспользуемся формулой

Тогда реализация такого соотношения будет выглядеть следующим образом:


void QuaternionToMatrix(float m[4][4], const Quaternion * quat)
{
  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
  x2 = quat->x + quat->x;
  y2 = quat->y + quat->y;
  z2 = quat->z + quat->z;
  xx = quat->x * x2;   xy = quat->x * y2;   xz = quat->x * z2;
  yy = quat->y * y2;   yz = quat->y * z2;   zz = quat->z * z2;
  wx = quat->w * x2;   wy = quat->w * y2;   wz = quat->w * z2;

  m[0][0]=1.0f-(yy+zz); m[0][1]=xy-wz;        m[0][2]=xz+wy;
  m[1][0]=xy+wz;        m[1][1]=1.0f-(xx+zz); m[1][2]=yz-wx;
  m[2][0]=xz-wy;        m[2][1]=yz+wx;        m[2][2]=1.0f-(xx+yy);

  m[0][3] = m[1][3] = m[2][3] = 0;
  m[3][0] = m[3][1] = m[3][2] = 0;
  m[3][3] = 1;
}

Теперь построим обратное отображение aij матрица поворота, и w² + x² + y² + z² = 1, тогда

Знаки кватерниона можно выбрать и другими способами что продемонстрировано в листинге ниже


void QuaternionNormalize(Quaternion * quat) {
  float magnitude;
  
  magnitude = sqrt((quat->x * quat->x) + (quat->y * quat->y) + (quat->z * quat->z) + (quat->w * quat->w));
  quat->x /= magnitude;
  quat->y /= magnitude;
  quat->z /= magnitude;
  quat->w /= magnitude;
}

Quaternion  MatrixToQuaternion( float m[4][4])
{	
Quaternion result;

    result->w= ( m[0][0] + m[1][1]  + m[2][2]   + 1.0f) / 4.0f;
    result->x = (m[0][0]  - m[1][1]   - m[2][2]   + 1.0f) / 4.0f;
    result->y = (-m[0][0]  + m[1][1]   - m[2][2]   + 1.0f) / 4.0f;
    result->z = (-m[0][0]  - m[1][1]   + m[2][2]   + 1.0f) / 4.0f;
    if(result->w < 0.0f) q0 = 0.0f;
    if(result->x < 0.0f) q1 = 0.0f;
    if(result->y < 0.0f) q2 = 0.0f;
    if(result->z < 0.0f) q3 = 0.0f;
    result->w = sqrt(result->w);
    result->x = sqrt(result->x);
    result->y = sqrt(result->y);
    result->z = sqrt(result->z);
    if(result->w >= result->x && result->w >= result->y && result->w >= result->z) {
            result->w *= +1.0f;
            result->x *= SIGN(m[2][1]   - m[1][2]  );
            result->y *= SIGN(m[0][2]   - m[2][0]  );
           result->z *= SIGN(m[1][0]  - m[0][1]  );
    } else if(result->x >= result->w && result->x >= result->y && result->x >= result->z) {
            result->w *= SIGN(m[2][1]   - m[1][2]  );
            result->x *= +1.0f;
            result->y *= SIGN(m[1][0]  + m[0][1]  );
            result->z *= SIGN(m[0][2]   + m[2][0]  );
    } else if(result->y >= result->w && result->y >= result->x && result->y >= result->z) {
            result->w *= SIGN(m[0][2]   + m[2][0]  );
            result->x *= SIGN(m[1][0]  + m[0][1]  );
            result->y *= +1.0f;
            result->z *= SIGN(m[2][1]   + m[1][2]  );
    } else  {
            result->w *= SIGN(m[1][0]  - m[0][1]  );
            result->x *= SIGN(m[0][2]   + m[2][0]  );
            result->y *= SIGN(m[2][1]   + m[1][2]  );
            result->z *= +1.0f;
    }  
    QuaternionNormalize (result);
    return result;

}

Кватернионы и вращение.

Теперь, когда мы разобрались, с API, которые поддерживают только матрицы поворота, мы можем работать только с кватернионами.

Для начала реализуем функцию перемножения кватернионов.


Quaternion QuaternionMultiply(Quaternion * quat1, Quaternion * quat2) {
  Vector vector1, vector2, cross;
  Quaternion result;
  float angle;
  
  vector1.x = quat1->x;
  vector1.y = quat1->y;
  vector1.z = quat1->z;
  vector2   .x = quat2->x;
  vector2.y = quat2->y;
  vector2.z = quat2->z;
  angle = ((quat1->w * quat2->w) - (VectorDot(vector1, vector2)));
  
  cross = VectorCross(vector1, vector2);
  vector1.x *= quat2->w;
  vector1.y *= quat2->w;
  vector1.z *= quat2->w;
  vector2.x *= quat1->w;
  vector2.y *= quat1->w;
  vector2.z *= quat1->w;
  
  result.x = (vector1.x + vector2.x + cross.x);
  result.y = (vector1.y + vector2.y + cross.y);
  result.z = (vector1.z + vector2.z + cross.z);
  result.w = angle;
  
  return result;
}

Теперь получим функцию обратного кватерниона:



void QuaternionInvert(Quaternion * quat) {
  float length;
  
  length = (1.0f / ((quat->x * quat->x) +
                    (quat->y * quat->y) +
                    (quat->z * quat->z) +
                    (quat->w * quat->w)));
  quat->x *= -length;
  quat->y *= -length;
  quat->z *= -length;
  quat->w *= length;
}

И наконец, функция, которая позволяет повернуть вектор на вращение заданное, кватернионом:



Vector QuaternionMultiplyVector(Quaternion * quat, Vector * vector) {
  Quaternion vectorQuat, inverseQuat, resultQuat;
  Vector resultVector;
  
  vectorQuat.x = vector->x;
  vectorQuat.y = vector->y;
  vectorQuat.z = vector->z;
  vectorQuat.w = 0.0f;
  
  inverseQuat = *quat;
  QuaternionInvert(&inverseQuat);
  resultQuat = QuaternionMultiply(&vectorQuat, &inverseQuat);
  resultQuat = QuaternionMultiply(quat, &resultQuat);
  
  resultVector.x = resultQuat.x;
  resultVector.y = resultQuat.y;
  resultVector.z = resultQuat.z;
  
  return resultVector;
}

P.S. В третей части я расскажу про плюсы и минусы использования кватернионов. И приведу примеры функций полезных для работы с кватернионами.
В этой статье использовались материалы, полученные из следующих источников
Convert from rotation matrix to quaternion;
Кватернионы;
Кватернионы в программировании игр.