#ifndef UNIT_VECTOR_HH
#define UNIT_VECTOR_HH
#include <dune/common/fvector.hh>
#include <dune/common/fmatrix.hh>
#include <dune/common/version.hh>
#include <dune/common/math.hh>
#include <dune/gfe/tensor3.hh>
#include <dune/gfe/symmetricmatrix.hh>
template <class T, int N>
class Rotation;
/** \brief A unit vector in R^N
\tparam N Dimension of the embedding space
\tparam T The type used for individual coordinates
*/
template <class T, int N>
class UnitVector
{
// Rotation<T,3> is friend, because it needs the various derivatives of the arccos
friend class Rotation<T,3>;
/** \brief Computes sin(x) / x without getting unstable for small x */
static T sinc(const T& x) {
using std::sin;
return (x < 1e-2) ? 1.0-x*x/6.0+ Dune::power(x,4)/120.0-Dune::power(x,6)/5040.0+Dune::power(x,8)/362880.0 : sin(x)/x;
}
/** \brief Compute arccos^2 without using the (at 1) nondifferentiable function acos x close to 1 */
static T arcCosSquared(const T& x) {
using std::acos;
const T eps = 1e-2;
if (x > 1-eps) { // acos is not differentiable, use the series expansion instead,
// we need here lots of terms to be sure that the numerical derivatives are also within maschine precision
//return -2 * (x-1) + 1.0/3 * (x-1)*(x-1) - 4.0/45 * (x-1)*(x-1)*(x-1);
return 11665028.0/4729725.0
-141088.0/45045.0*x
+ 413.0/429.0*x*x
- 5344.0/12285.0*Dune::power(x,3)
+ 245.0/1287.0*Dune::power(x,4)
- 1632.0/25025.0*Dune::power(x,5)
+ 56.0/3861.0*Dune::power(x,6)
- 32.0/21021.0*Dune::power(x,7);
} else {
return Dune::power(acos(x),2);
}
}
/** \brief Compute the derivative of arccos^2 without getting unstable for x close to 1 */
static T derivativeOfArcCosSquared(const T& x) {
using std::acos;
using std::sqrt;
const T eps = 1e-2;
if (x > 1-eps) { // regular expression is unstable, use the series expansion instead
// we need here lots of terms to be sure that the numerical derivatives are also within maschine precision
//return -2 + 2*(x-1)/3 - 4/15*(x-1)*(x-1);
return -47104.0/15015.0
+12614.0/6435.0*x
-63488.0/45045.0*x*x
+ 1204.0/1287.0*Dune::power(x,3)
- 2048.0/4095.0*Dune::power(x,4)
+ 112.0/585.0*Dune::power(x,5)
-2048.0/45045.0*Dune::power(x,6)
+ 32.0/6435.0*Dune::power(x,7);
} else if (x < -1+eps) { // The function is not differentiable
DUNE_THROW(Dune::Exception, "arccos^2 is not differentiable at x==-1!");
} else
return -2*acos(x) / sqrt(1-x*x);
}
/** \brief Compute the second derivative of arccos^2 without getting unstable for x close to 1 */
static T secondDerivativeOfArcCosSquared(const T& x) {
using std::acos;
using std::pow;
const T eps = 1e-2;
if (x > 1-eps) { // regular expression is unstable, use the series expansion instead
// we need here lots of terms to be sure that the numerical derivatives are also within maschine precision
//return 2.0/3 - 8*(x-1)/15;
return 1350030.0/676039.0+5632.0/2028117.0*Dune::power(x,10)
-1039056896.0/334639305.0*x
+150876.0/39767.0*x*x
-445186048.0/111546435.0*Dune::power(x,3)
+ 343728.0/96577.0*Dune::power(x,4)
- 57769984.0/22309287.0*Dune::power(x,5)
+ 710688.0/482885.0*Dune::power(x,6)
- 41615360.0/66927861.0*Dune::power(x,7)
+ 616704.0/3380195.0*Dune::power(x,8)
- 245760.0/7436429.0*Dune::power(x,9);
} else if (x < -1+eps) { // The function is not differentiable
DUNE_THROW(Dune::Exception, "arccos^2 is not differentiable at x==-1!");
} else
return 2/(1-x*x) - 2*x*acos(x) / pow(1-x*x,1.5);
}
/** \brief Compute the third derivative of arccos^2 without getting unstable for x close to 1 */
static T thirdDerivativeOfArcCosSquared(const T& x) {
using std::acos;
using std::sqrt;
const T eps = 1e-2;
if (x > 1-eps) { // regular expression is unstable, use the series expansion instead
// we need here lots of terms to be sure that the numerical derivatives are also within maschine precision
//return -8.0/15 + 24*(x-1)/35;
return -1039056896.0/334639305.0
+301752.0/39767.0*x
-445186048.0/37182145.0*x*x
+1374912.0/96577.0*Dune::power(x,3)
-288849920.0/22309287.0*Dune::power(x,4)
+4264128.0/482885.0*Dune::power(x,5)
-41615360.0/9561123.0*Dune::power(x,6)
+4933632.0/3380195.0*Dune::power(x,7)
-2211840.0/7436429.0*Dune::power(x,8)
+56320.0/2028117.0*Dune::power(x,9);
} else if (x < -1+eps) { // The function is not differentiable
DUNE_THROW(Dune::Exception, "arccos^2 is not differentiable at x==-1!");
} else {
T d = 1-x*x;
return 6*x/(d*d) - 6*x*x*acos(x)/(d*d*sqrt(d)) - 2*acos(x)/(d*sqrt(d));
}
}
template <class T2, int N2>
friend class UnitVector;
public:
/** \brief The type used for coordinates */
typedef T ctype;
typedef T field_type;
/** \brief The type used for global coordinates */
typedef Dune::FieldVector<T,N> CoordinateType;
/** \brief Dimension of the manifold formed by unit vectors */
static const int dim = N-1;
/** \brief Dimension of the Euclidean space the manifold is embedded in */
static const int embeddedDim = N;
typedef Dune::FieldVector<T,N-1> TangentVector;
typedef Dune::FieldVector<T,N> EmbeddedTangentVector;
/** \brief The global convexity radius of the unit sphere */
static constexpr double convexityRadius = 0.5*M_PI;
/** \brief The return type of the derivativeOfProjection method */
typedef Dune::FieldMatrix<T, N, N> DerivativeOfProjection;
/** \brief Default constructor */
UnitVector()
{}
/** \brief Constructor from a vector. The vector gets normalized! */
UnitVector(const Dune::FieldVector<T,N>& vector)
: data_(vector)
{
data_ /= data_.two_norm();
}
/** \brief Constructor from an array. The array gets normalized! */
UnitVector(const std::array<T,N>& vector)
{
for (int i=0; i<N; i++)
data_[i] = vector[i];
data_ /= data_.two_norm();
}
/** \brief Assignment from UnitVector with different type -- used for automatic differentiation with ADOL-C */
template <class T2>
UnitVector& operator <<= (const UnitVector<T2,N>& other) {
for (int i=0; i<N; i++)
data_[i] <<= other.data_[i];
return *this;
}
/** \brief Rebind the UnitVector to another coordinate type */
template<class U>
struct rebind
{
typedef UnitVector<U,N> other;
};
UnitVector<T,N>& operator=(const Dune::FieldVector<T,N>& vector)
{
data_ = vector;
data_ /= data_.two_norm();
return *this;
}
/** \brief The exponential map */
static UnitVector exp(const UnitVector& p, const TangentVector& v) {
Dune::FieldMatrix<T,N-1,N> frame = p.orthonormalFrame();
EmbeddedTangentVector ev;
frame.mtv(v,ev);
return exp(p,ev);
}
/** \brief The exponential map */
static UnitVector exp(const UnitVector& p, const EmbeddedTangentVector& v) {
using std::abs;
using std::cos;
assert( abs(p.data_*v) < 1e-5 );
const T norm = v.two_norm();
UnitVector result = p;
result.data_ *= cos(norm);
result.data_.axpy(sinc(norm), v);
return result;
}
/** \brief The inverse of the exponential map
*
* \results A vector in the tangent space of p
*/
static EmbeddedTangentVector log(const UnitVector& p, const UnitVector& q)
{
EmbeddedTangentVector result = p.projectOntoTangentSpace(q.data_-p.data_);
if (result.two_norm() > 1e-10)
result *= distance(p,q) / result.two_norm();
return result;
}
/** \brief Length of the great arc connecting the two points */
static T distance(const UnitVector& a, const UnitVector& b) {
using std::acos;
using std::min;
// Not nice: we are in a class for unit vectors, but the class is actually
// supposed to handle perturbations of unit vectors as well. Therefore
// we normalize here.
T x = a.data_ * b.data_/a.data_.two_norm()/b.data_.two_norm();
// paranoia: if the argument is just eps larger than 1 acos returns NaN
x = min(x,1.0);
return acos(x);
}
#if ADOLC_ADOUBLE_H
/** \brief Squared length of the great arc connecting the two points */
static adouble distanceSquared(const UnitVector<double,N>& a, const UnitVector<adouble,N>& b)
{
// Not nice: we are in a class for unit vectors, but the class is actually
// supposed to handle perturbations of unit vectors as well. Therefore
// we normalize here.
adouble x = a.data_ * b.data_ / (a.data_.two_norm()*b.data_.two_norm());
// paranoia: if the argument is just eps larger than 1 acos returns NaN
using std::min;
x = min(x,1.0);
// Special implementation that remains AD-differentiable near x==1
return arcCosSquared(x);
}
#endif
/** \brief Compute the gradient of the squared distance function keeping the first argument fixed
Unlike the distance itself the squared distance is differentiable at zero
*/
static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const UnitVector& a, const UnitVector& b) {
T x = a.data_ * b.data_;
EmbeddedTangentVector result = a.data_;
result *= derivativeOfArcCosSquared(x);
// Project gradient onto the tangent plane at b in order to obtain the surface gradient
result = b.projectOntoTangentSpace(result);
// Gradient must be a tangent vector at b, in other words, orthogonal to it
using std::abs;
assert(abs(b.data_ * result) < 1e-5);
return result;
}
/** \brief Compute the Hessian of the squared distance function keeping the first argument fixed
Unlike the distance itself the squared distance is differentiable at zero
*/
static Dune::SymmetricMatrix<T,N> secondDerivativeOfDistanceSquaredWRTSecondArgument(const UnitVector& p, const UnitVector& q) {
T sp = p.data_ * q.data_;
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
Dune::SymmetricMatrix<T,N> A;
for (int i=0; i<N; i++)
for (int j=0; j<=i; j++)
A(i,j) = pProjected[i]*pProjected[j];
A *= secondDerivativeOfArcCosSquared(sp);
// Compute matrix B (see notes)
Dune::SymmetricMatrix<T,N> Pq;
for (int i=0; i<N; i++)
for (int j=0; j<=i; j++)
Pq(i,j) = (i==j) - q.data_[i]*q.data_[j];
// Bring it all together
A.axpy(-1*derivativeOfArcCosSquared(sp)*sp, Pq);
return A;
}
/** \brief Compute the mixed second derivate \partial d^2 / \partial da db
Unlike the distance itself the squared distance is differentiable at zero
*/
static Dune::FieldMatrix<T,N,N> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const UnitVector& a, const UnitVector& b) {
T sp = a.data_ * b.data_;
// Compute vector A (see notes)
Dune::FieldMatrix<T,1,N> row;
row[0] = b.projectOntoTangentSpace(a.globalCoordinates());
Dune::FieldVector<T,N> tmp = a.projectOntoTangentSpace(b.globalCoordinates());
Dune::FieldMatrix<T,N,1> column;
for (int i=0; i<N; i++) // turn row vector into column vector
column[i] = tmp[i];
Dune::FieldMatrix<T,N,N> A;
// A = row * column
Dune::FMatrixHelp::multMatrix(column,row,A);
A *= secondDerivativeOfArcCosSquared(sp);
// Compute matrix B (see notes)
Dune::FieldMatrix<T,N,N> Pp, Pq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++) {
Pp[i][j] = (i==j) - a.data_[i]*a.data_[j];
Pq[i][j] = (i==j) - b.data_[i]*b.data_[j];
}
Dune::FieldMatrix<T,N,N> B;
Dune::FMatrixHelp::multMatrix(Pp,Pq,B);
// Bring it all together
A.axpy(derivativeOfArcCosSquared(sp), B);
return A;
}
/** \brief Compute the third derivative \partial d^3 / \partial dq^3
Unlike the distance itself the squared distance is differentiable at zero
*/
static Tensor3<T,N,N,N> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const UnitVector& p, const UnitVector& q) {
Tensor3<T,N,N,N> result;
T sp = p.data_ * q.data_;
// The projection matrix onto the tangent space at p and q
Dune::FieldMatrix<T,N,N> Pq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
Pq[i][j] = (i==j) - q.globalCoordinates()[i]*q.globalCoordinates()[j];
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
for (int k=0; k<N; k++) {
result[i][j][k] = thirdDerivativeOfArcCosSquared(sp) * pProjected[i] * pProjected[j] * pProjected[k]
- secondDerivativeOfArcCosSquared(sp) * ((i==j)*sp + p.globalCoordinates()[i]*q.globalCoordinates()[j])*pProjected[k]
- secondDerivativeOfArcCosSquared(sp) * ((i==k)*sp + p.globalCoordinates()[i]*q.globalCoordinates()[k])*pProjected[j]
- secondDerivativeOfArcCosSquared(sp) * pProjected[i] * Pq[j][k] * sp
+ derivativeOfArcCosSquared(sp) * ((i==j)*q.globalCoordinates()[k] + (i==k)*q.globalCoordinates()[j]) * sp
- derivativeOfArcCosSquared(sp) * p.globalCoordinates()[i] * Pq[j][k];
}
result = Pq * result;
return result;
}
/** \brief Compute the mixed third derivative \partial d^3 / \partial da db^2
Unlike the distance itself the squared distance is differentiable at zero
*/
static Tensor3<T,N,N,N> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const UnitVector& p, const UnitVector& q) {
Tensor3<T,N,N,N> result;
T sp = p.data_ * q.data_;
// The projection matrix onto the tangent space at p and q
Dune::FieldMatrix<T,N,N> Pp, Pq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++) {
Pp[i][j] = (i==j) - p.globalCoordinates()[i]*p.globalCoordinates()[j];
Pq[i][j] = (i==j) - q.globalCoordinates()[i]*q.globalCoordinates()[j];
}
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
Dune::FieldVector<T,N> qProjected = p.projectOntoTangentSpace(q.globalCoordinates());
Tensor3<T,N,N,N> derivativeOfPqOTimesPq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
for (int k=0; k<N; k++) {
derivativeOfPqOTimesPq[i][j][k] = 0;
for (int l=0; l<N; l++)
derivativeOfPqOTimesPq[i][j][k] += Pp[i][l] * (Pq[j][l]*pProjected[k] + pProjected[j]*Pq[k][l]);
}
result = thirdDerivativeOfArcCosSquared(sp) * Tensor3<T,N,N,N>::product(qProjected,pProjected,pProjected)
+ secondDerivativeOfArcCosSquared(sp) * derivativeOfPqOTimesPq
- secondDerivativeOfArcCosSquared(sp) * sp * Tensor3<T,N,N,N>::product(qProjected,Pq)
- derivativeOfArcCosSquared(sp) * Tensor3<T,N,N,N>::product(qProjected,Pq);
return result;
}
/** \brief Project tangent vector of R^n onto the tangent space */
EmbeddedTangentVector projectOntoTangentSpace(const EmbeddedTangentVector& v) const {
EmbeddedTangentVector result = v;
result.axpy(-1*(data_*result), data_);
return result;
}
/** \brief Project tangent vector of R^n onto the normal space space */
EmbeddedTangentVector projectOntoNormalSpace(const EmbeddedTangentVector& v) const {
EmbeddedTangentVector result;
T sp = 0;
for (int i=0; i<N; i++)
sp += v[i] * data_[i];
for (int i=0; i<N; i++)
result[i] = sp * data_[i];
return result;
}
/** \brief The Weingarten map */
EmbeddedTangentVector weingarten(const EmbeddedTangentVector& z, const EmbeddedTangentVector& v) const {
EmbeddedTangentVector result;
T sp = 0;
for (int i=0; i<N; i++)
sp += v[i] * data_[i];
for (int i=0; i<N; i++)
result[i] = -sp * z[i];
return result;
}
static UnitVector<T,N> projectOnto(const CoordinateType& p)
{
UnitVector<T,N> result(p);
result.data_ /= result.data_.two_norm();
return result;
}
static DerivativeOfProjection derivativeOfProjection(const Dune::FieldVector<T,N>& p)
{
auto normSquared = p.two_norm2();
auto norm = std::sqrt(normSquared);
Dune::FieldMatrix<T,N,N> result;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
result[i][j] = ( (i==j) - p[i]*p[j] / normSquared ) / norm;
return result;
}
/** \brief The global coordinates, if you really want them */
const CoordinateType& globalCoordinates() const {
return data_;
}
/** \brief Compute an orthonormal basis of the tangent space of S^n.
This basis is of course not globally continuous.
*/
Dune::FieldMatrix<T,N-1,N> orthonormalFrame() const {
Dune::FieldMatrix<T,N-1,N> result;
// Coordinates of the stereographic projection
Dune::FieldVector<T,N-1> X;
if (data_[N-1] <= 0) {
// Stereographic projection from the north pole onto R^{N-1}
for (size_t i=0; i<N-1; i++)
X[i] = data_[i] / (1-data_[N-1]);
} else {
// Stereographic projection from the south pole onto R^{N-1}
for (size_t i=0; i<N-1; i++)
X[i] = data_[i] / (1+data_[N-1]);
}
T RSquared = X.two_norm2();
for (size_t i=0; i<N-1; i++)
for (size_t j=0; j<N-1; j++)
// Note: the matrix is the transpose of the one in the paper
result[j][i] = 2*(i==j)*(1+RSquared) - 4*X[i]*X[j];
for (size_t j=0; j<N-1; j++)
result[j][N-1] = 4*X[j];
// Upper hemisphere: adapt formulas so it is the stereographic projection from the south pole
if (data_[N-1] > 0)
for (size_t j=0; j<N-1; j++)
result[j][N-1] *= -1;
// normalize the rows to make the orthogonal basis orthonormal
for (size_t i=0; i<N-1; i++)
result[i] /= result[i].two_norm();
return result;
}
/** \brief Write unit vector object to output stream */
friend std::ostream& operator<< (std::ostream& s, const UnitVector& unitVector)
{
return s << unitVector.data_;
}
private:
Dune::FieldVector<T,N> data_;
};
#endif