📄 geometry.pas
字号:
XS := Q.Vector[X] * S; YS := Q.Vector[Y] * S; ZS := Q.Vector[Z] * S;
WX := Q.RealPart * XS; WY := Q.RealPart * YS; WZ := Q.RealPart * ZS;
XX := Q.Vector[X] * XS; XY := Q.Vector[X] * YS; XZ := Q.Vector[X] * ZS;
YY := Q.Vector[Y] * YS; YZ := Q.Vector[Y] * ZS; ZZ := Q.Vector[Z] * ZS;
Result[X, X] := 1 - (YY + ZZ); Result[Y, X] := XY + WZ; Result[Z, X] := XZ - WY; Result[W, X] := 0;
Result[X, Y] := XY - WZ; Result[Y, Y] := 1 - (XX + ZZ); Result[Z, Y] := YZ + WX; Result[W, Y] := 0;
Result[X, Z] := XZ + WY; Result[Y, Z] := YZ - WX; Result[Z, Z] := 1 - (XX + YY); Result[W, Z] := 0;
Result[X, W] := 0; Result[Y, W] := 0; Result[Z, W] := 0; Result[W, W] := 1;}
var
V: TAffineVector;
SinA, CosA,
A, B, C: Extended;
begin
V := Q.ImagPart;
VectorNormalize(V);
SinCos(Q.RealPart / 2, SinA, CosA);
A := V[X] * SinA;
B := V[Y] * SinA;
C := V[Z] * SinA;
Result := IdentityMatrix;
Result[X, X] := 1 - 2 * B * B - 2 * C * C;
Result[X, Y] := 2 * A * B - 2 * CosA * C;
Result[X, Z] := 2 * A * C + 2 * CosA * B;
Result[Y, X] := 2 * A * B + 2 * CosA * C;
Result[Y, Y] := 1 - 2 * A * A - 2 * C * C;
Result[Y, Z] := 2 * B * C - 2 * CosA * A;
Result[Z, X] := 2 * A * C - 2 * CosA * B;
Result[Z, Y] := 2 * B * C + 2 * CosA * A;
Result[Z, Z] := 1 - 2 * A * A - 2 * B * B;
end;
//----------------------------------------------------------------------------------------------------------------------
procedure QuaternionToPoints(Q: TQuaternion; var ArcFrom, ArcTo: TAffineVector); register;
// converts a unit quaternion into two points on a unit sphere
var S: Single;
begin
S := Sqrt(Q.ImagPart[X] * Q.ImagPart[X] + Q.ImagPart[Y] * Q.ImagPart[Y]);
if S = 0 then ArcFrom := MakeAffineVector([0, 1, 0])
else ArcFrom := MakeAffineVector([-Q.ImagPart[Y] / S, Q.ImagPart[X] / S, 0]);
ArcTo[X] := Q.RealPart * ArcFrom[X] - Q.ImagPart[Z] * ArcFrom[Y];
ArcTo[Y] := Q.RealPart * ArcFrom[Y] + Q.ImagPart[Z] * ArcFrom[X];
ArcTo[Z] := Q.ImagPart[X] * ArcFrom[Y] - Q.ImagPart[Y] * ArcFrom[X];
if Q.RealPart < 0 then ArcFrom := MakeAffineVector([-ArcFrom[X], -ArcFrom[Y], 0]);
end;
//----------------------------------------------------------------------------------------------------------------------
function MatrixAffineDeterminant(M: TAffineMatrix): Single; register;
// determinant of a 3x3 matrix
begin
Result := M[X, X] * (M[Y, Y] * M[Z, Z] - M[Z, Y] * M[Y, Z]) -
M[X, Y] * (M[Y, X] * M[Z, Z] - M[Z, X] * M[Y, Z]) +
M[X, Z] * (M[Y, X] * M[Z, Y] - M[Z, X] * M[Y, Y]);
end;
//----------------------------------------------------------------------------------------------------------------------
function MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3: Single): Single;
// internal version for the determinant of a 3x3 matrix
begin
Result := a1 * (b2 * c3 - b3 * c2) -
b1 * (a2 * c3 - a3 * c2) +
c1 * (a2 * b3 - a3 * b2);
end;
//----------------------------------------------------------------------------------------------------------------------
procedure MatrixAdjoint(var M: TMatrix); register;
// Adjoint of a 4x4 matrix - used in the computation of the inverse
// of a 4x4 matrix
var a1, a2, a3, a4,
b1, b2, b3, b4,
c1, c2, c3, c4,
d1, d2, d3, d4: Single;
begin
a1 := M[X, X]; b1 := M[X, Y];
c1 := M[X, Z]; d1 := M[X, W];
a2 := M[Y, X]; b2 := M[Y, Y];
c2 := M[Y, Z]; d2 := M[Y, W];
a3 := M[Z, X]; b3 := M[Z, Y];
c3 := M[Z, Z]; d3 := M[Z, W];
a4 := M[W, X]; b4 := M[W, Y];
c4 := M[W, Z]; d4 := M[W, W];
// row column labeling reversed since we transpose rows & columns
M[X, X] := MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4);
M[Y, X] := -MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4);
M[Z, X] := MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4);
M[W, X] := -MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
M[X, Y] := -MatrixDetInternal(b1, b3, b4, c1, c3, c4, d1, d3, d4);
M[Y, Y] := MatrixDetInternal(a1, a3, a4, c1, c3, c4, d1, d3, d4);
M[Z, Y] := -MatrixDetInternal(a1, a3, a4, b1, b3, b4, d1, d3, d4);
M[W, Y] := MatrixDetInternal(a1, a3, a4, b1, b3, b4, c1, c3, c4);
M[X, Z] := MatrixDetInternal(b1, b2, b4, c1, c2, c4, d1, d2, d4);
M[Y, Z] := -MatrixDetInternal(a1, a2, a4, c1, c2, c4, d1, d2, d4);
M[Z, Z] := MatrixDetInternal(a1, a2, a4, b1, b2, b4, d1, d2, d4);
M[W, Z] := -MatrixDetInternal(a1, a2, a4, b1, b2, b4, c1, c2, c4);
M[X, W] := -MatrixDetInternal(b1, b2, b3, c1, c2, c3, d1, d2, d3);
M[Y, W] := MatrixDetInternal(a1, a2, a3, c1, c2, c3, d1, d2, d3);
M[Z, W] := -MatrixDetInternal(a1, a2, a3, b1, b2, b3, d1, d2, d3);
M[W, W] := MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3);
end;
//----------------------------------------------------------------------------------------------------------------------
function MatrixDeterminant(M: TMatrix): Single; register;
// Determinant of a 4x4 matrix
var a1, a2, a3, a4,
b1, b2, b3, b4,
c1, c2, c3, c4,
d1, d2, d3, d4 : Single;
begin
a1 := M[X, X]; b1 := M[X, Y]; c1 := M[X, Z]; d1 := M[X, W];
a2 := M[Y, X]; b2 := M[Y, Y]; c2 := M[Y, Z]; d2 := M[Y, W];
a3 := M[Z, X]; b3 := M[Z, Y]; c3 := M[Z, Z]; d3 := M[Z, W];
a4 := M[W, X]; b4 := M[W, Y]; c4 := M[W, Z]; d4 := M[W, W];
Result := a1 * MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
b1 * MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
c1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
d1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
end;
//----------------------------------------------------------------------------------------------------------------------
procedure MatrixScale(var M: TMatrix; Factor: Single); register;
// multiplies all elements of a 4x4 matrix with a factor
var I, J: Integer;
begin
for I := 0 to 3 do
for J := 0 to 3 do M[I, J] := M[I, J] * Factor;
end;
//----------------------------------------------------------------------------------------------------------------------
procedure MatrixInvert(var M: TMatrix); register;
// finds the inverse of a 4x4 matrix
var Det: Single;
begin
Det := MatrixDeterminant(M);
if Abs(Det) < EPSILON then M := IdentityMatrix
else
begin
MatrixAdjoint(M);
MatrixScale(M, 1 / Det);
end;
end;
//----------------------------------------------------------------------------------------------------------------------
procedure MatrixTranspose(var M: TMatrix); register;
// computes transpose of 4x4 matrix
var I, J: Integer;
TM: TMatrix;
begin
for I := 0 to 3 do
for J := 0 to 3 do TM[J, I] := M[I, J];
M := TM;
end;
//----------------------------------------------------------------------------------------------------------------------
procedure MatrixAffineTranspose(var M: TAffineMatrix); register;
// computes transpose of 3x3 matrix
var I, J: Integer;
TM: TAffineMatrix;
begin
for I := 0 to 2 do
for J := 0 to 2 do TM[J, I] := M[I, J];
M := TM;
end;
//----------------------------------------------------------------------------------------------------------------------
function MatrixMultiply(M1, M2: TMatrix): TMatrix; register;
// multiplies two 4x4 matrices
var I, J: Integer;
TM: TMatrix;
begin
for I := 0 to 3 do
for J := 0 to 3 do
TM[I, J] := M1[I, X] * M2[X, J] +
M1[I, Y] * M2[Y, J] +
M1[I, Z] * M2[Z, J] +
M1[I, W] * M2[W, J];
Result := TM;
end;
//----------------------------------------------------------------------------------------------------------------------
function CreateRotationMatrix(Axis: TVector3f; Angle: Single): TMatrix; register;
// Creates a rotation matrix along the given Axis by the given Angle in radians.
var cosine,
sine,
Len,
one_minus_cosine: Extended;
begin
SinCos(Angle, Sine, Cosine);
one_minus_cosine := 1 - cosine;
Len := VectorNormalize(Axis);
if Len = 0 then Result := IdentityMatrix
else
begin
Result[X, X] := (one_minus_cosine * Sqr(Axis[0])) + Cosine;
Result[X, Y] := (one_minus_cosine * Axis[0] * Axis[1]) - (Axis[2] * Sine);
Result[X, Z] := (one_minus_cosine * Axis[2] * Axis[0]) + (Axis[1] * Sine);
Result[X, W] := 0;
Result[Y, X] := (one_minus_cosine * Axis[0] * Axis[1]) + (Axis[2] * Sine);
Result[Y, Y] := (one_minus_cosine * Sqr(Axis[1])) + Cosine;
Result[Y, Z] := (one_minus_cosine * Axis[1] * Axis[2]) - (Axis[0] * Sine);
Result[Y, W] := 0;
Result[Z, X] := (one_minus_cosine * Axis[2] * Axis[0]) - (Axis[1] * Sine);
Result[Z, Y] := (one_minus_cosine * Axis[1] * Axis[2]) + (Axis[0] * Sine);
Result[Z, Z] := (one_minus_cosine * Sqr(Axis[2])) + Cosine;
Result[Z, W] := 0;
Result[W, X] := 0;
Result[W, Y] := 0;
Result[W, Z] := 0;
Result[W, W] := 1;
end;
end;
//----------------------------------------------------------------------------------------------------------------------
function ConvertRotation(Angles: TAffineVector): TVector; register;
{ Turn a triplet of rotations about x, y, and z (in that order) into an
equivalent rotation around a single axis (all in radians).
Rotation of the Angle t about the axis (X, Y, Z) is given by:
| X^2 + (1-X^2) Cos(t), XY(1-Cos(t)) + Z Sin(t), XZ(1-Cos(t))-Y Sin(t) |
M = | XY(1-Cos(t))-Z Sin(t), Y^2 + (1-Y^2) Cos(t), YZ(1-Cos(t)) + X Sin(t) |
| XZ(1-Cos(t)) + Y Sin(t), YZ(1-Cos(t))-X Sin(t), Z^2 + (1-Z^2) Cos(t) |
Rotation about the three axes (Angles a1, a2, a3) can be represented as
the product of the individual rotation matrices:
| 1 0 0 | | Cos(a2) 0 -Sin(a2) | | Cos(a3) Sin(a3) 0 |
| 0 Cos(a1) Sin(a1) | * | 0 1 0 | * | -Sin(a3) Cos(a3) 0 |
| 0 -Sin(a1) Cos(a1) | | Sin(a2) 0 Cos(a2) | | 0 0 1 |
Mx My Mz
We now want to solve for X, Y, Z, and t given 9 equations in 4 unknowns.
Using the diagonal elements of the two matrices, we get:
X^2 + (1-X^2) Cos(t) = M[0][0]
Y^2 + (1-Y^2) Cos(t) = M[1][1]
Z^2 + (1-Z^2) Cos(t) = M[2][2]
Adding the three equations, we get:
X^2 + Y^2 + Z^2 - (M[0][0] + M[1][1] + M[2][2]) =
- (3 - X^2 - Y^2 - Z^2) Cos(t)
Since (X^2 + Y^2 + Z^2) = 1, we can rewrite as:
Cos(t) = (1 - (M[0][0] + M[1][1] + M[2][2])) / 2
Solving for t, we get:
t = Acos(((M[0][0] + M[1][1] + M[2][2]) - 1) / 2)
We can substitute t into the equations for X^2, Y^2, and Z^2 above
to get the values for X, Y, and Z. To find the proper signs we note
that:
2 X Sin(t) = M[1][2] - M[2][1]
2 Y Sin(t) = M[2][0] - M[0][2]
2 Z Sin(t) = M[0][1] - M[1][0]
}
var Axis1, Axis2: TVector3f;
M, M1, M2: TMatrix;
cost, cost1,
sint,
s1, s2, s3: Single;
I: Integer;
begin
// see if we are only rotating about a single Axis
if Abs(Angles[X]) < EPSILON then
begin
if Abs(Angles[Y]) < EPSILON then
begin
Result := MakeVector([0, 0, 1, Angles[Z]]);
Exit;
end
else
if Abs(Angles[Z]) < EPSILON then
begin
Result := MakeVector([0, 1, 0, Angles[Y]]);
Exit;
end
end
else
if (Abs(Angles[Y]) < EPSILON) and
(Abs(Angles[Z]) < EPSILON) then
begin
Result := MakeVector([1, 0, 0, Angles[X]]);
Exit;
end;
// make the rotation matrix
Axis1 := MakeAffineVector([1, 0, 0]);
M := CreateRotationMatrix(Axis1, Angles[X]);
Axis2 := MakeAffineVector([0, 1, 0]);
M2 := CreateRotationMatrix(Axis2, Angles[Y]);
M1 := MatrixMultiply(M, M2);
Axis2 := MakeAffineVector([0, 0, 1]);
M2 := CreateRotationMatrix(Axis2, Angles[Z]);
M := MatrixMultiply(M1, M2);
cost := ((M[X, X] + M[Y, Y] + M[Z, Z])-1) / 2;
if cost < -1 then cost := -1
else
if cost > 1 - EPSILON then
begin
// Bad Angle - this would cause a crash
Result := MakeVector([1, 0, 0, 0]);
Exit;
end;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -