Code:
//--------------------------------------------------------------------------
const
AsphyreEpsilon = 0.0000001;
//--------------------------------------------------------------------------
function QuaternionToMatrix3(const q: TVector4): TMatrix3;
begin
Result[0, 0]:= 1.0 - (2.0 * q.y * q.y) - (2.0 * q.z * q.z);
Result[0, 1]:= (2.0 * q.x * q.y) + (2.0 * q.w * q.z);
Result[0, 2]:= (2.0 * q.x * q.z) - (2.0 * q.w * q.y);
Result[1, 0]:= (2.0 * q.x * q.y) - (2.0 * q.w * q.z);
Result[1, 1]:= 1.0 - (2.0 * q.x * q.x) - (2.0 * q.z * q.z);
Result[1, 2]:= (2.0 * q.y * q.z) + (2.0 * q.w * q.x);
Result[2, 0]:= (2.0 * q.x * q.z) + (2.0 * q.w * q.y);
Result[2, 1]:= (2.0 * q.y * q.z) - (2.0 * q.w * q.x);
Result[2, 2]:= 1.0 - (2.0 * q.x * q.x) - (2.0 * q.y * q.y);
end;
//--------------------------------------------------------------------------
function MatrixToQuaternion3(const Matrix: TMatrix3): TVector4;
var
Aux : TVector4;
Max : Single;
Index: Integer;
High : Double;
Mult : Double;
begin
// Determine which of w, x, y, z has the largest absolute value.
Aux.w:= Matrix[0, 0] + Matrix[1, 1] + Matrix[2, 2];
Aux.x:= Matrix[0, 0] - Matrix[1, 1] - Matrix[2, 2];
Aux.y:= Matrix[1, 1] - Matrix[0, 0] - Matrix[2, 2];
Aux.z:= Matrix[2, 2] - Matrix[0, 0] - Matrix[1, 1];
Index:= 0;
Max := Aux.w;
if (Aux.x > Max) then
begin
Max := Aux.x;
Index:= 1;
end;
if (Aux.y > Max) then
begin
Max := Aux.y;
Index:= 2;
end;
if (Aux.z > Max) then
begin
Max := Aux.z;
Index:= 3;
end;
// Perform square root and division.
High:= Sqrt(Max + 1.0) * 0.5;
Mult:= 0.25 / High;
// Apply table to compute quaternion values.
case Index of
0: begin
Result.w:= High;
Result.x:= (Matrix[1, 2] - Matrix[2, 1]) * Mult;
Result.y:= (Matrix[2, 0] - Matrix[0, 2]) * Mult;
Result.z:= (Matrix[0, 1] - Matrix[1, 0]) * Mult;
end;
1: begin
Result.x:= High;
Result.w:= (Matrix[1, 2] - Matrix[2, 1]) * Mult;
Result.z:= (Matrix[2, 0] + Matrix[0, 2]) * Mult;
Result.y:= (Matrix[0, 1] + Matrix[1, 0]) * Mult;
end;
2: begin
Result.y:= High;
Result.z:= (Matrix[1, 2] + Matrix[2, 1]) * Mult;
Result.w:= (Matrix[2, 0] - Matrix[0, 2]) * Mult;
Result.x:= (Matrix[0, 1] + Matrix[1, 0]) * Mult;
end;
else
begin
Result.z:= High;
Result.y:= (Matrix[1, 2] + Matrix[2, 1]) * Mult;
Result.x:= (Matrix[2, 0] + Matrix[0, 2]) * Mult;
Result.w:= (Matrix[0, 1] - Matrix[1, 0]) * Mult;
end;
end;
end;
//--------------------------------------------------------------------------
function QuaternionSlerp(const q1, q2: TVector4; Theta: Single): TVector4;
var
CosOmega, SinOmega: Real;
q: TVector4;
Omega: Real;
Coef0, Coef1: Single;
begin
// Compute the "cosine of the angle" between the quaternions,
// using the dot product.
CosOmega:= q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
// If negative dot, negate one of the input
// quaterions to take the shorter 4D "arc".
q:= q2;
if (CosOmega < 0) then
begin
CosOmega:= -CosOmega;
q.x:= -q2.x;
q.y:= -q2.y;
q.z:= -q2.z;
q.w:= -q2.w;
end;
// Check if they are very close together to protect against divide-by-zero.
Coef0:= 1.0 - Theta;
Coef1:= Theta;
if (1.0 - CosOmega > AsphyreEpsilon) then
begin
// Spherical interpolation.
Omega:= ArcCos(CosOmega);
SinOmega:= Sin(Omega);
Coef0:= Sin((1.0 - Theta) * Omega) / SinOmega;
Coef1:= Sin(Theta * Omega) / SinOmega;
end;
// Interpolate.
Result.x:= (Coef0 * q1.x) + (Coef1 * q.x);
Result.y:= (Coef0 * q1.y) + (Coef1 * q.y);
Result.z:= (Coef0 * q1.z) + (Coef1 * q.z);
Result.w:= (Coef0 * q1.w) + (Coef1 * q.w);
end;
Bookmarks