Определение угловой скорости, необходимой для регулировки ориентации на основе кватернионов

Question

Проблема:

У меня есть объект в трехмерном пространстве, который существует в данной ориентации. Мне нужно переориентировать объект на новую ориентацию. В настоящее время я представляю ориентации как кватернионы, хотя это не является строго необходимым.

По сути, мне нужно определить угловую скорость, необходимую для ориентации тела в нужной ориентации.

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

Psuedocode:

// 4x4 Matrix containing rotation and translation
Matrix4 currentTransform = GetTransform();

// Grab the 3x3 matrix containing orientation only
Matrix3 currentOrientMtx = currentTransform.Get3x3();

// Build a quat based on the rotation matrix
Quaternion currentOrientation(currentOrientMtx);
currentOrientation.Normalize();

// Build a new matrix describing our desired orientation
Vector3f zAxis = desiredForward;
Vector3f yAxis = desiredUp;
Vector3f xAxis = yAxis.Cross(zAxis);
Matrix3 desiredOrientMtx(xAxis, yAxis, zAxis);

// Build a quat from our desired roation matrix
Quaternion desiredOrientation(desiredOrientMtx);
desiredOrientation.Normalize();

// Slerp from our current orientation to the new orientation based on our turn rate and time delta
Quaternion slerpedQuat = currentOrientation.Slerp(desiredOrientation, turnRate * deltaTime);

// Determine the axis and angle of rotation
Vector3f rotationAxis = slerpedQuat.GetAxis();
float rotationAngle = slerpedQuat.GetAngle();

// Determine angular displacement and angular velocity
Vector3f angularDisplacement = rotationAxis * rotationAngle;    
Vector3f angularVelocity = angularDisplacement / deltaTime;

SetAngularVelocity(angularVelocity);

По сути, это просто отправляет мой объект в забвение. Я убедился, что требуемый OrientMtx, который я построил по осям, действительно является правильным окончательным преобразованием вращения. Я чувствую, что мне здесь не хватает чего-то глупого.

Мысли?

Answer 1

Чтобы рассчитать угловую скорость, ваш turnRateуже обеспечивает величину (рад / сек), так что все, что вам действительно нужно, это ось вращения. Это просто дано GetAxis( B * Inverse(A) ), GetAngle из того же количества даст общий угол для перемещения между ними. См. "Разница" между двумя кватернионами для дальнейшего объяснения.

SetAngularVelocity( Normalize( GetAxis( B * Inverse(A)) ) * turnRate )

Вам нужно установить угловую скорость на 0 в некоторой точке (когда вы достигнете своей цели ориентации). Один из способов сделать это - использовать кватернионное расстояние. Другим более простым способом является проверка количества времени. Наконец, вы можете проверить угол между двумя кватами (как обсуждалось выше) и проверить, близок ли он к 0.

float totalAngle = GetAngle( Normalize( endingPose * Inverse( startingPose ) ) );
if( fabs( totalAngle ) > 0.0001 )  // some epsilon
{
     // your setting angular velocity code here 
     SetAngularVelocity(angularVelocity);
}
else
{
     SetAngularVelocity( Vector3f(0) );
     // Maybe, if you want high accuracy, call SetTransform here too
}

Но, на самом деле, я не понимаю, почему вы не просто используете Slerp в полном объеме. Вместо того, чтобы полагаться на интегратор физики (что может быть неточным) и полагаться на знание того, когда вы достигли своего пункта назначения (что несколько неловко), вы можете просто перемещать объект кадр за кадром, так как вы знаете движение.

Quaternion startingPose;
Quaternion endingPose;
// As discussed earlier...
Quaternion totalAngle = Quaternion.AngleBetween( startingPose, endingPose ); 

// t is set to 0 whenever you start a new motion
t += deltaT;

float howFarIn = (turnRate * t) / totalAngle; 
SetCurrentTransform( startingPose.Slerp( endingPose, howFarIn ) );

См. Плавное вращение с кватернионами для некоторого обсуждения этого.