You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
570 lines
18 KiB
570 lines
18 KiB
// David Eberly, Geometric Tools, Redmond WA 98052
|
|
// Copyright (c) 1998-2021
|
|
// Distributed under the Boost Software License, Version 1.0.
|
|
// https://www.boost.org/LICENSE_1_0.txt
|
|
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
|
// Version: 4.8.2020.08.11
|
|
|
|
// Rotation matrices can be constructed using estimates of the coefficients
|
|
// that involve trigonometric and polynomial terms. See
|
|
// https://www.geometrictools.com/Documentation/RotationEstimation.pdf
|
|
// for the length details.
|
|
|
|
#pragma once
|
|
|
|
#include <Mathematics/Matrix.h>
|
|
#include <array>
|
|
|
|
namespace gte
|
|
{
|
|
// Constants for rotc0(t) = sin(t)/t.
|
|
std::array<std::array<double, 9>, 7> constexpr C_ROTC0_EST_COEFF =
|
|
{ {
|
|
{ // degree 4
|
|
+1.00000000000000000e+00,
|
|
-1.58971650732578684e-01,
|
|
+5.84121356311684790e-03
|
|
},
|
|
{ // degree 6
|
|
+1.00000000000000000e+00,
|
|
-1.66218398161274539e-01,
|
|
+8.06129151017077016e-03,
|
|
-1.50545944866583496e-04
|
|
},
|
|
{ // degree 8
|
|
+1.00000000000000000e+00,
|
|
-1.66651290458553397e-01,
|
|
+8.31836205080888937e-03,
|
|
-1.93853969255209339e-04,
|
|
+2.19921657358978346e-06
|
|
},
|
|
{ // degree 10
|
|
+1.00000000000000000e+00,
|
|
-1.66666320608302304e-01,
|
|
+8.33284074932796014e-03,
|
|
-1.98184457544372085e-04,
|
|
+2.70931602688878442e-06,
|
|
-2.07033154672609224e-08
|
|
},
|
|
{ // degree 12
|
|
+1.00000000000000000e+00,
|
|
-1.66666661172424985e-01,
|
|
+8.33332258782319701e-03,
|
|
-1.98405693280704135e-04,
|
|
+2.75362742468406608e-06,
|
|
-2.47308402190765123e-08,
|
|
+1.36149932075244694e-10
|
|
},
|
|
{ // degree 14
|
|
+1.00000000000000000e+00,
|
|
-1.66666666601880786e-01,
|
|
+8.33333316679120591e-03,
|
|
-1.98412553530683797e-04,
|
|
+2.75567210003238900e-06,
|
|
-2.50388692626200884e-08,
|
|
+1.58972932135933544e-10,
|
|
-6.61111627233688785e-13
|
|
},
|
|
{ // degree 16
|
|
+1.00000000000000000e+00,
|
|
-1.66666666666648478e-01,
|
|
+8.33333333318112164e-03,
|
|
-1.98412698077537775e-04,
|
|
+2.75573162083557394e-06,
|
|
-2.50519743096581360e-08,
|
|
+1.60558314470477309e-10,
|
|
-7.60488921303402553e-13,
|
|
+2.52255089807125025e-15
|
|
}
|
|
} };
|
|
|
|
std::array<double, 7> constexpr C_ROTC0_EST_MAX_ERROR =
|
|
{
|
|
6.9656371186750e-03, // degree 4
|
|
2.2379506089580e-04, // degree 6
|
|
4.8670096434722e-06, // degree 8
|
|
7.5654711606532e-08, // degree 10
|
|
8.7939172610518e-10, // degree 12
|
|
7.9199615615755e-12, // degree 14
|
|
6.8001160258291e-16 // degree 16
|
|
};
|
|
|
|
// Constants for rotc1(t) = (1-cos(t))/t^2.
|
|
std::array<std::array<double, 9>, 7> constexpr C_ROTC1_EST_COEFF =
|
|
{ {
|
|
{ // degree 4
|
|
+5.00000000000000000e-01,
|
|
-4.06593520914583922e-02,
|
|
+1.06698549928666312e-03
|
|
},
|
|
{ // degree 6
|
|
+5.00000000000000000e-01,
|
|
-4.16202835017619524e-02,
|
|
+1.36087417563353699e-03,
|
|
-1.99122437404000405e-05
|
|
},
|
|
{ // degree 8
|
|
+5.00000000000000000e-01,
|
|
-4.16653520191245796e-02,
|
|
+1.38761160375298095e-03,
|
|
-2.44138380330618480e-05,
|
|
+2.28499434819148172e-07
|
|
},
|
|
{ // degree 10
|
|
+5.00000000000000000e-01,
|
|
-4.16666414534321572e-02,
|
|
+1.38885303988537192e-03,
|
|
-2.47850001122705350e-05,
|
|
+2.72207208413898425e-07,
|
|
-1.77358008600681907e-09
|
|
},
|
|
{ // degree 12
|
|
+5.00000000000000000e-01,
|
|
-4.16666663178411334e-02,
|
|
+1.38888820709641924e-03,
|
|
-2.48011431705518285e-05,
|
|
+2.75439902962340229e-07,
|
|
-2.06736081122602257e-09,
|
|
+9.93003618302030503e-12
|
|
},
|
|
{ // degree 14
|
|
+5.00000000000000000e-01,
|
|
-4.16666666664263635e-02,
|
|
+1.38888888750799658e-03,
|
|
-2.48015851902670717e-05,
|
|
+2.75571871163332658e-07,
|
|
-2.08727380201649381e-09,
|
|
+1.14076763269827225e-11,
|
|
-4.28619236995285237e-14
|
|
},
|
|
{ // degree 16
|
|
+5.00000000000000000e-01,
|
|
-4.16666666666571719e-02,
|
|
+1.38888888885105744e-03,
|
|
-2.48015872513761947e-05,
|
|
+2.75573160474227648e-07,
|
|
-2.08766469798137579e-09,
|
|
+1.14685460418668139e-11,
|
|
-4.75415775440997119e-14,
|
|
+1.40555891469552795e-16
|
|
}
|
|
} };
|
|
|
|
std::array<double, 7> constexpr C_ROTC1_EST_MAX_ERROR =
|
|
{
|
|
9.2119010150538e-04, // degree 4
|
|
2.3251261806301e-05, // degree 6
|
|
4.1693160884870e-07, // degree 8
|
|
5.5177887536839e-09, // degree 10
|
|
5.5865700954172e-11, // degree 12
|
|
7.1609385088323e-15, // degree 14
|
|
7.2164496600635e-16 // degree 16
|
|
};
|
|
|
|
// Constants for rotc2(t) = (sin(t) - t*cos(t))/t^3.
|
|
std::array<std::array<double, 9>, 7> constexpr C_ROTC2_EST_COEFF =
|
|
{ {
|
|
{ // degree 4
|
|
+3.33333333333333315e-01,
|
|
-3.24417271573718483e-02,
|
|
+9.05201583387763454e-04
|
|
},
|
|
{ // degree 6
|
|
+3.33333333333333315e-01,
|
|
-3.32912781805089902e-02,
|
|
+1.16506615743456146e-03,
|
|
-1.76083105011587047e-05
|
|
},
|
|
{ // degree 8
|
|
+3.33333333333333315e-01,
|
|
-3.33321218985461534e-02,
|
|
+1.18929901553194335e-03,
|
|
-2.16884239911580259e-05,
|
|
+2.07111898922214621e-07
|
|
},
|
|
{ // degree 10
|
|
+3.33333333333333315e-01,
|
|
-3.33333098285273563e-02,
|
|
+1.19044276839748377e-03,
|
|
-2.20303898188601926e-05,
|
|
+2.47382309397892291e-07,
|
|
-1.63412179599052932e-09
|
|
},
|
|
{ // degree 12
|
|
+3.33333333333333315e-01,
|
|
-3.33333330053029661e-02,
|
|
+1.19047554930589209e-03,
|
|
-2.20454376925152508e-05,
|
|
+2.50395723787030737e-07,
|
|
-1.90797721719554658e-09,
|
|
+9.25661051509749896e-12
|
|
},
|
|
{ // degree 14
|
|
+3.33333333333333315e-01,
|
|
-3.33333333331133561e-02,
|
|
+1.19047618918715682e-03,
|
|
-2.20458533943125258e-05,
|
|
+2.50519837811549507e-07,
|
|
-1.92670551155064303e-09,
|
|
+1.06463697865186991e-11,
|
|
-4.03135292145519115e-14
|
|
},
|
|
{ // degree 16
|
|
+3.33333333333333315e-01,
|
|
-3.33333333333034956e-02,
|
|
+1.19047619036920628e-03,
|
|
-2.20458552540489507e-05,
|
|
+2.50521015434838418e-07,
|
|
-1.92706504721931338e-09,
|
|
+1.07026043656398707e-11,
|
|
-4.46498739610373537e-14,
|
|
+1.30526089083317312e-16
|
|
}
|
|
} };
|
|
|
|
std::array<double, 7> constexpr C_ROTC2_EST_MAX_ERROR =
|
|
{
|
|
8.1461508460229e-04, // degree 4
|
|
2.1075025784856e-05, // degree 6
|
|
3.8414838612888e-07, // degree 8
|
|
5.1435967152180e-09, // degree 10
|
|
5.2533588590364e-11, // degree 12
|
|
7.7715611723761e-15, // degree 14
|
|
2.2759572004816e-15 // degree 16
|
|
};
|
|
|
|
// Constants for rotc3(t) = (2*(1-cos(t)) - t*sin(t))/t^4.
|
|
std::array<std::array<double, 9>, 7> constexpr C_ROTC3_EST_COEFF =
|
|
{ {
|
|
{ // degree 4
|
|
+8.33333333333333287e-02,
|
|
-5.46357009138465424e-03,
|
|
+1.19638433962248889e-04
|
|
},
|
|
{ // degree 6
|
|
+8.33333333333333287e-02,
|
|
-5.55196372993948303e-03,
|
|
+1.46646667516630680e-04,
|
|
-1.82905866698780768e-06
|
|
},
|
|
{ // degree 8
|
|
+8.33333333333333287e-02,
|
|
-5.55546733314307706e-03,
|
|
+1.48723933698110248e-04,
|
|
-2.17865651989456709e-06,
|
|
+1.77408035681006169e-08
|
|
},
|
|
{ // degree 10
|
|
+8.33333333333333287e-02,
|
|
-5.55555406357728914e-03,
|
|
+1.48807404153008735e-04,
|
|
-2.20360578108261882e-06,
|
|
+2.06782449582308932e-08,
|
|
-1.19178562817913197e-10
|
|
},
|
|
{ // degree 12
|
|
+8.33333333333333287e-02,
|
|
-5.55555555324832757e-03,
|
|
+1.48809514798423797e-04,
|
|
-2.20457622072950518e-06,
|
|
+2.08728631685852690e-08,
|
|
-1.36888190776165574e-10,
|
|
+5.99292681875750821e-13
|
|
},
|
|
{ // degree 14
|
|
+8.33333333333333287e-02,
|
|
-5.55555555528319030e-03,
|
|
+1.48809523101214977e-04,
|
|
-2.20458493798151629e-06,
|
|
+2.08765224186559757e-08,
|
|
-1.37600800115177215e-10,
|
|
+6.63762129016229865e-13,
|
|
-2.19044013684859942e-15
|
|
},
|
|
{ // degree 16
|
|
+8.33333333333333287e-02,
|
|
-5.55555555501025672e-03,
|
|
+1.48809521898935978e-04,
|
|
-2.20458342827337994e-06,
|
|
+2.08757075326674457e-08,
|
|
-1.37379825035843510e-10,
|
|
+6.32209097599974706e-13,
|
|
+7.39204014316007136e-17,
|
|
-6.43236558920699052e-17
|
|
}
|
|
} };
|
|
|
|
std::array<double, 7> constexpr C_ROTC3_EST_MAX_ERROR =
|
|
{
|
|
8.4612036888886e-05, // degree 4
|
|
1.8051973185995e-06, // degree 6
|
|
2.8016103950645e-08, // degree 8
|
|
3.2675415151395e-10, // degree 10
|
|
1.3714029911682e-13, // degree 12
|
|
3.2078506517763e-14, // degree 14
|
|
4.7774284528401e-14 // degree 16
|
|
};
|
|
}
|
|
|
|
namespace gte
|
|
{
|
|
// Estimate rotc0(t) = sin(t)/t for t in [0,pi]. For example, a degree-6
|
|
// estimate is
|
|
// float t; // in [0,pi]
|
|
// float result = RotC0Estimate<float, 6>(t);
|
|
template <typename Real, size_t Degree>
|
|
inline Real RotC0Estimate(Real t)
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
|
|
size_t constexpr select = (Degree - 4) / 2;
|
|
auto constexpr& coeff = C_ROTC0_EST_COEFF[select];
|
|
size_t constexpr last = Degree / 2;
|
|
Real tsqr = t * t;
|
|
Real poly = static_cast<Real>(coeff[last]);
|
|
for (size_t i = 0, index = last - 1; i < last; ++i, --index)
|
|
{
|
|
poly = static_cast<Real>(coeff[index]) + poly * tsqr;
|
|
}
|
|
return poly;
|
|
}
|
|
|
|
// Estimate rotc1(t) = (1 - cos(t))/t^2 for t in [0,pi]. For example,
|
|
// a degree-6 estimate is
|
|
// float t; // in [0,pi]
|
|
// float result = RotC1Estimate<float, 6>(t);
|
|
template <typename Real, size_t Degree>
|
|
inline Real RotC1Estimate(Real t)
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
|
|
size_t constexpr select = (Degree - 4) / 2;
|
|
auto constexpr& coeff = C_ROTC1_EST_COEFF[select];
|
|
size_t constexpr last = Degree / 2;
|
|
Real tsqr = t * t;
|
|
Real poly = static_cast<Real>(coeff[last]);
|
|
for (size_t i = 0, index = last - 1; i < last; ++i, --index)
|
|
{
|
|
poly = static_cast<Real>(coeff[index]) + poly * tsqr;
|
|
}
|
|
return poly;
|
|
}
|
|
|
|
// Estimate rotc2(t) = (sin(t) - t*cos(t))/t^3 for t in [0,pi]. For
|
|
// example, a degree-6 estimate is
|
|
// float t; // in [0,pi]
|
|
// float result = RotC2Estimate<float, 6>(t);
|
|
template <typename Real, size_t Degree>
|
|
inline Real RotC2Estimate(Real t)
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
|
|
size_t constexpr select = (Degree - 4) / 2;
|
|
auto constexpr& coeff = C_ROTC2_EST_COEFF[select];
|
|
size_t constexpr last = Degree / 2;
|
|
Real tsqr = t * t;
|
|
Real poly = static_cast<Real>(coeff[last]);
|
|
for (size_t i = 0, index = last - 1; i < last; ++i, --index)
|
|
{
|
|
poly = static_cast<Real>(coeff[index]) + poly * tsqr;
|
|
}
|
|
return poly;
|
|
}
|
|
|
|
// Estimate rotc3(t) = (2*(1-cos(t)) - t*sin(t))/t^4 for t in
|
|
// [0,pi]. For example, a degree-6 estimate is
|
|
// float t; // in [0,pi]
|
|
// float result = RotC3Estimate<float, 6>(t);
|
|
template <typename Real, size_t Degree>
|
|
inline Real RotC3Estimate(Real t)
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
|
|
size_t constexpr select = (Degree - 4) / 2;
|
|
auto constexpr& coeff = C_ROTC3_EST_COEFF[select];
|
|
size_t constexpr last = Degree / 2;
|
|
Real tsqr = t * t;
|
|
Real poly = static_cast<Real>(coeff[last]);
|
|
for (size_t i = 0, index = last - 1; i < last; ++i, --index)
|
|
{
|
|
poly = static_cast<Real>(coeff[index]) + poly * tsqr;
|
|
}
|
|
return poly;
|
|
}
|
|
|
|
template <typename Real, size_t Degree>
|
|
Real constexpr GetRotC0EstimateMaxError()
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
return static_cast<Real>(C_ROTC0_EST_MAX_ERROR[(Degree - 4) / 2]);
|
|
}
|
|
|
|
template <typename Real, size_t Degree>
|
|
Real constexpr GetRotC1EstimateMaxError()
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
return static_cast<Real>(C_ROTC1_EST_MAX_ERROR[(Degree - 4) / 2]);
|
|
}
|
|
|
|
template <typename Real, size_t Degree>
|
|
Real constexpr GetRotC2EstimateMaxError()
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
return static_cast<Real>(C_ROTC2_EST_MAX_ERROR[(Degree - 4) / 2]);
|
|
}
|
|
|
|
template <typename Real, size_t Degree>
|
|
Real constexpr GetRotC3EstimateMaxError()
|
|
{
|
|
static_assert((Degree & 1) == 0 && 4 <= Degree && Degree <= 16, "Invalid degree.");
|
|
return static_cast<Real>(C_ROTC3_EST_MAX_ERROR[(Degree - 4) / 2]);
|
|
}
|
|
|
|
// Construct the estimate for the rotation matrix
|
|
// R = exp(S) = I + rotc0(t) * S + rotc1(t) * S^2
|
|
// from a vector (p0,p1,p2) with length t = |(p0,p1,p2)| and
|
|
// skew-symmetric matrix S = {{0,-p2,p1},{p2,0,-p0},{-p1,p0,0}}.
|
|
template <typename Real, size_t Degree>
|
|
void RotationEstimate(Vector<3, Real> const& p,
|
|
Matrix<3, 3, Real>& R)
|
|
{
|
|
Real const zero(0), one(1);
|
|
|
|
Matrix<3, 3, Real> I{
|
|
one, zero, zero,
|
|
zero, one, zero,
|
|
zero, zero, one
|
|
};
|
|
|
|
Matrix<3, 3, Real> S{
|
|
zero, -p[2], p[1],
|
|
p[2], zero, -p[0],
|
|
-p[1], p[0], zero
|
|
};
|
|
|
|
Real p0p0 = p[0] * p[0], p0p1 = p[0] * p[1], p0p2 = p[0] * p[2];
|
|
Real p1p1 = p[1] * p[1], p1p2 = p[1] * p[2], p2p2 = p[2] * p[2];
|
|
Matrix<3, 3, Real> Ssqr{
|
|
-(p1p1 + p2p2), p0p1, p0p2,
|
|
p0p1, -(p0p0 + p2p2), p1p2,
|
|
p0p2, p1p2, -(p0p0 + p1p1)
|
|
};
|
|
|
|
Real t = Length(p);
|
|
Real a = RotC0Estimate<Real, Degree>(t);
|
|
Real b = RotC1Estimate<Real, Degree>(t);
|
|
R = I + a * S + b * Ssqr;
|
|
};
|
|
|
|
template <typename Real, size_t Degree>
|
|
void RotationDerivativeEstimate(Vector<3, Real> const& p,
|
|
std::array<Matrix<3, 3, Real>, 3>& Rder)
|
|
{
|
|
Real const zero(0), one(1), negOne(-1);
|
|
|
|
std::array<Matrix<3, 3, Real>, 3> skewE =
|
|
{
|
|
Matrix<3, 3, Real>{
|
|
zero, zero, zero,
|
|
zero, zero, negOne,
|
|
zero, one, zero
|
|
},
|
|
|
|
Matrix<3, 3, Real>{
|
|
zero, zero, one,
|
|
zero, zero, zero,
|
|
negOne, zero, zero
|
|
},
|
|
|
|
Matrix<3, 3, Real>{
|
|
zero, negOne, zero,
|
|
one, zero, zero,
|
|
zero, zero, zero
|
|
}
|
|
};
|
|
|
|
Matrix<3, 3, Real> S{
|
|
zero, -p[2], p[1],
|
|
p[2], zero, -p[0],
|
|
-p[1], p[0], zero
|
|
};
|
|
|
|
Real p0p0 = p[0] * p[0], p0p1 = p[0] * p[1], p0p2 = p[0] * p[2];
|
|
Real p1p1 = p[1] * p[1], p1p2 = p[1] * p[2], p2p2 = p[2] * p[2];
|
|
Matrix<3, 3, Real> Ssqr{
|
|
-(p1p1 + p2p2), p0p1, p0p2,
|
|
p0p1, -(p0p0 + p2p2), p1p2,
|
|
p0p2, p1p2, -(p0p0 + p1p1)
|
|
};
|
|
|
|
Real t = Length(p);
|
|
Real a = RotC0Estimate<Real, Degree>(t);
|
|
Real b = RotC1Estimate<Real, Degree>(t);
|
|
Real c = RotC2Estimate<Real, Degree>(t);
|
|
Real d = RotC3Estimate<Real, Degree>(t);
|
|
for (int32_t i = 0; i < 3; ++i)
|
|
{
|
|
Rder[i] = a * skewE[i] + b * (S * skewE[i] + skewE[i] * S) - p[i] * (c * S + d * Ssqr);
|
|
}
|
|
}
|
|
|
|
template <typename Real, size_t Degree>
|
|
void RotationAndDerivativeEstimate(Vector<3, Real> const& p,
|
|
Matrix<3, 3, Real>& R, std::array<Matrix<3, 3, Real>, 3>& Rder)
|
|
{
|
|
Real const zero(0), one(1), negOne(-1);
|
|
|
|
Matrix<3, 3, Real> I{
|
|
one, zero, zero,
|
|
zero, one, zero,
|
|
zero, zero, one
|
|
};
|
|
|
|
std::array<Matrix<3, 3, Real>, 3> skewE =
|
|
{
|
|
Matrix<3, 3, Real>{
|
|
zero, zero, zero,
|
|
zero, zero, negOne,
|
|
zero, one, zero
|
|
},
|
|
|
|
Matrix<3, 3, Real>{
|
|
zero, zero, one,
|
|
zero, zero, zero,
|
|
negOne, zero, zero
|
|
},
|
|
|
|
Matrix<3, 3, Real>{
|
|
zero, negOne, zero,
|
|
one, zero, zero,
|
|
zero, zero, zero
|
|
}
|
|
};
|
|
|
|
Matrix<3, 3, Real> S{
|
|
zero, -p[2], p[1],
|
|
p[2], zero, -p[0],
|
|
-p[1], p[0], zero
|
|
};
|
|
|
|
Real p0p0 = p[0] * p[0], p0p1 = p[0] * p[1], p0p2 = p[0] * p[2];
|
|
Real p1p1 = p[1] * p[1], p1p2 = p[1] * p[2], p2p2 = p[2] * p[2];
|
|
Matrix<3, 3, Real> Ssqr{
|
|
-(p1p1 + p2p2), p0p1, p0p2,
|
|
p0p1, -(p0p0 + p2p2), p1p2,
|
|
p0p2, p1p2, -(p0p0 + p1p1)
|
|
};
|
|
|
|
Real t = Length(p);
|
|
Real a = RotC0Estimate<Real, Degree>(t);
|
|
Real b = RotC1Estimate<Real, Degree>(t);
|
|
Real c = RotC2Estimate<Real, Degree>(t);
|
|
Real d = RotC3Estimate<Real, Degree>(t);
|
|
|
|
R = I + a * S + b * Ssqr;
|
|
for (int32_t i = 0; i < 3; ++i)
|
|
{
|
|
Rder[i] = a * skewE[i] + b * (S * skewE[i] + skewE[i] * S) - p[i] * (c * S + d * Ssqr);
|
|
}
|
|
}
|
|
}
|
|
|