SO3Util.cpp

 
#include "tools/SO3Util.h"
 
 
/* Retourne la matrice de rotation de la rotation (a.b).
 */
void SO3Util::sO3FromTwoVec(Vector3d a, Vector3d b, Matrix3x3d &result)
{
  Vector3d sO3FromTwoVecN;
  Vector3d sO3FromTwoVecRotationAxis;
  Vector3d sO3FromTwoVecA;
  Vector3d sO3FromTwoVecB;
 
  Vector3d::cross(a, b, sO3FromTwoVecN);
  if (sO3FromTwoVecN.length() == 0.0)
  {
    double dot = Vector3d::dot(a, b);
    if (dot >= 0.0)
    {
      result.setIdentity();
    }
    else
    {
      Vector3d::ortho(a, sO3FromTwoVecRotationAxis);
      rotationPiAboutAxis(sO3FromTwoVecRotationAxis, result);
    }
    return;
  }
 
  sO3FromTwoVecA.set(a);
  sO3FromTwoVecB.set(b);
 
  sO3FromTwoVecN.normalize();
  sO3FromTwoVecA.normalize();
  sO3FromTwoVecB.normalize();
 
  Matrix3x3d r1;
  r1.setColumn(0, sO3FromTwoVecA);
  r1.setColumn(1, sO3FromTwoVecN);
 
  Vector3d temp31;
  Vector3d::cross(sO3FromTwoVecN, sO3FromTwoVecA, temp31);
  r1.setColumn(2, temp31);
 
  Matrix3x3d r2;
  r2.setColumn(0, sO3FromTwoVecB);
  r2.setColumn(1, sO3FromTwoVecN);
  Vector3d::cross(sO3FromTwoVecN, sO3FromTwoVecB, temp31);
  r2.setColumn(2, temp31);
 
  r1.transpose();
  Matrix3x3d::mult(r2, r1, result);
}
 
/* Retourne la matrice de rotation d'une rotation autour de v de 180° (= PI)
 */
void SO3Util::rotationPiAboutAxis(Vector3d v, Matrix3x3d &result)
{
  Vector3d rotationPiAboutAxisTemp;
  rotationPiAboutAxisTemp.set(v);
  rotationPiAboutAxisTemp.scale(3.141592653589793 / rotationPiAboutAxisTemp.length());
  double invTheta = 0.3183098861837907;
  double kA = 0.0;
  double kB = 0.2026423672846756;
  rodriguesSo3Exp(rotationPiAboutAxisTemp, kA, kB, result);
}
 
/* retourne la matrice de rotation qui correspond à un vecteur
 * Dans un soucis d'optimisation, on stocke les rotations sous forme de vecteur.
 * Question: comment est stocké l'angle? à rechercher!!!
 */
void SO3Util::sO3FromMu(Vector3d w, Matrix3x3d &result)
{
  double thetaSq = Vector3d::dot(w, w);
  double theta = sqrt(thetaSq);
  double kA, kB;
  if (thetaSq < 1.0E-08)
  {
    kA = 1.0 - 0.16666667163372 * thetaSq;
    kB = 0.5;
  }
  else
  {
    if (thetaSq < 1.0E-06)
    {
      kB = 0.5 - 0.0416666679084301 * thetaSq;
      kA = 1.0 - thetaSq * 0.16666667163372 * (1.0 - 0.16666667163372 * thetaSq);
    }
    else
    {
      double invTheta = 1.0 / theta;
      kA = sin(theta) * invTheta;
      kB = (1.0 - cos(theta)) * (invTheta * invTheta);
    }
  }
  rodriguesSo3Exp(w, kA, kB, result);
}
 
/* retourne le vecteur qui correspond à la matrice de rotation
 * Dans un soucis d'optimisation, on stocke les rotations sous forme de vecteur.
 * Question: comment est stocké l'angle? à rechercher!!!
 */
void SO3Util::muFromSO3(Matrix3x3d so3, Vector3d &result)
{
  double cosAngle = (so3.get(0, 0) + so3.get(1, 1) + so3.get(2, 2) - 1.0) * 0.5;
 
  result.set((so3.get(2, 1) - so3.get(1, 2)) / 2.0, (so3.get(0, 2) - so3.get(2, 0)) / 2.0, (so3.get(1, 0) - so3.get(0, 1)) / 2.0);
 
  double sinAngleAbs = result.length();
  if (cosAngle > 0.7071067811865476)
  {
    if (sinAngleAbs > 0.0)
      result.scale(asin(sinAngleAbs) / sinAngleAbs);
  }
  else if (cosAngle > -0.7071067811865476)
  {
    double angle = acos(cosAngle);
    result.scale(angle / sinAngleAbs);
  }
  else
  {
    double angle = 3.141592653589793 - asin(sinAngleAbs);
    double d0 = so3.get(0, 0) - cosAngle;
    double d1 = so3.get(1, 1) - cosAngle;
    double d2 = so3.get(2, 2) - cosAngle;
 
    Vector3d r2;
    if ((d0 * d0 > d1 * d1) && (d0 * d0 > d2 * d2))
    {
      r2.set(d0, (so3.get(1, 0) + so3.get(0, 1)) / 2.0, (so3.get(0, 2) + so3.get(2, 0)) / 2.0);
    }
    else if (d1 * d1 > d2 * d2)
    {
      r2.set((so3.get(1, 0) + so3.get(0, 1)) / 2.0, d1, (so3.get(2, 1) + so3.get(1, 2)) / 2.0);
    }
    else
    {
      r2.set((so3.get(0, 2) + so3.get(2, 0)) / 2.0, (so3.get(2, 1) + so3.get(1, 2)) / 2.0, d2);
    }
 
    if (Vector3d::dot(r2, result) < 0.0)
    {
      r2.scale(-1.0);
    }
 
    r2.normalize();
    r2.scale(angle);
    result.set(r2);
  }
}
 
/* Formule de rodrigues.
 * Calcul la matrice de rotation (axe, angle)
 * Question: pourquoi ka et kb?
 */
void SO3Util::rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d &result)
{
  double wx2 = w.x * w.x;
  double wy2 = w.y * w.y;
  double wz2 = w.z * w.z;
 
  result.set(0, 0, 1.0 - kB * (wy2 + wz2));
  result.set(1, 1, 1.0 - kB * (wx2 + wz2));
  result.set(2, 2, 1.0 - kB * (wx2 + wy2));
 
  double a, b;
  a = kA * w.z;
  b = kB * (w.x * w.y);
  result.set(0, 1, b - a);
  result.set(1, 0, b + a);
 
  a = kA * w.y;
  b = kB * (w.x * w.z);
  result.set(0, 2, b + a);
  result.set(2, 0, b - a);
 
  a = kA * w.x;
  b = kB * (w.y * w.z);
  result.set(1, 2, b - a);
  result.set(2, 1, b + a);
}
 
void SO3Util::generatorField(int i, Matrix3x3d pos, Matrix3x3d &result)
{
  result.set(i, 0, 0.0);
  result.set((i + 1) % 3, 0, -pos.get((i + 2) % 3, 0));
  result.set((i + 2) % 3, 0, pos.get((i + 1) % 3, 0));
}