src/lib-gmapkernel/tools/geometry/geometry.icc

Go to the documentation of this file.

/*
 * lib-gmapkernel : Un noyau de 3-G-cartes et des opérations.
 * Copyright (C) 2004, Moka Team, Université de Poitiers, Laboratoire SIC
 *               http://www.sic.sp2mi.univ-poitiers.fr/
 * Copyright (C) 2009, Guillaume Damiand, CNRS, LIRIS,
 *               guillaume.damiand@liris.cnrs.fr, http://liris.cnrs.fr/
 *
 * This file is part of lib-gmapkernel
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

//******************************************************************************
#include "transformation-matrix.hh"
#include <cassert>
//******************************************************************************
INLINE
CVertex CGeometry::orthoProjectVertexOnLine(const CVertex& AVertex,
                                            const CVertex& ALineVertex,
                                            const CVertex& ALineDirection)
{
  assert(!ALineDirection.isNull());

  TCoordinate n2 = ALineDirection.sqrNorm();

  assert(!isZero(n2));

  TCoordinate t =
    ( ( AVertex.getX() - ALineVertex.getX() ) * ALineDirection.getX() +
      ( AVertex.getY() - ALineVertex.getY() ) * ALineDirection.getY() +
      ( AVertex.getZ() - ALineVertex.getZ() ) * ALineDirection.getZ() ) / n2;

  return ALineVertex + t*ALineDirection;
}
//******************************************************************************
INLINE
CVertex CGeometry::orthoProjectVertexOnPlane(const CVertex& AVertex,
                                             const TCoordinate& AA,
                                             const TCoordinate& AB,
                                             const TCoordinate& AC,
                                             const TCoordinate& AD)
{
  /* Le plan alpha pour équation A*x + B*y + C*z + D = 0.
   *
   * Soient X ,Y ,Z  les coordonnées du point à projeter (AVertex).
   * Soient XP,YP,ZP les coordonnées du point projeté sur le plan.
   * La droite orthogonale au plan et passant par AVertex est définie par
   * l'équation d'inconnues x y z et de paramètre t:
   *
   * (X-x)/A = (Y-y)/B = (Z-z)/C = t
   *
   * D'où:
   *
   * x = X + A*t
   * y = Y + B*t
   * z = Z + C*t
   *
   * Or le sommet (XP,YP,ZP) appartient à la droite et au plan. Donc:
   *
   * A*(X+A*t) + B*(Y+B*t) + C*(Z+C*t) + D = 0
   *
   * Cela permet de déterminer t:
   *
   * t = - (A*X + B*Y + C*Z + D) / (A^2 + B^2 + C^2)
   *
   * Et donc d'obtenir XP, YP et ZP...
   */

#define X (AVertex.getX())
#define Y (AVertex.getY())
#define Z (AVertex.getZ())

  TCoordinate t = - (AA*X + AB*Y + AC*Z + AD) / (sqr(AA) + sqr(AB) + sqr(AC));

  return CVertex(X + AA*t, Y + AB*t, Z + AC*t);

#undef X
#undef Y
#undef Z
}
//******************************************************************************
INLINE
CVertex CGeometry::orthoProjectVertexOnPlane(const CVertex& AVertex,
                                             const CVertex& APlaneVertex,
                                             const CVertex& APlaneNormal)
{
  assert(!APlaneNormal.isNull());

  /* Soient X , Y , Z  les coordonnées de AVertex.
   * Soient X0, Y0, Z0 les coordonnées de APlaneVertex.
   * Soient XN, YN, ZN les composantes de APlaneNormal.
   *
   * Le plan alpha pour équation A*x + B*y + C*z + D = 0, avec:
   *
   * A = XN
   * B = YN
   * C = ZN
   * D = - (A*X0 + B*Y0 + C*Z0)
   */

#define A (APlaneNormal.getX())
#define B (APlaneNormal.getY())
#define C (APlaneNormal.getZ())
#define D (- (A * APlaneVertex.getX() + \
              B * APlaneVertex.getY() + \
              C * APlaneVertex.getZ()))

  return orthoProjectVertexOnPlane(AVertex, A, B, C, D);

#undef A
#undef B
#undef C
#undef D
}

/* Autre écriture:
 *
 * AVertex +
 * APlaneNormal *
 * (APlaneVertex.dot(APlaneNormal) - AVertex.dot(APlaneNormal)) /
 * sqr(APlaneNormal.norm());
 */
//******************************************************************************
INLINE
CVertex CGeometry::centralHomothety(const CVertex& AVertex,
                                    const CVertex& ACenter,
                                    const TCoordinate& ACoef)
{
  return ACenter + ACoef*(AVertex-ACenter);
}
//******************************************************************************
INLINE
CVertex CGeometry::centralHomothety(const CVertex& AVertex,
                                    const CVertex& ACenter,
                                    const CVertex& ACoef)
{
  return ACenter + ACoef.multiply(AVertex-ACenter);
}
//******************************************************************************
INLINE
CVertex CGeometry::axialHomothety(const CVertex& AVertex,
                                  const CVertex& ALineVertex,
                                  const CVertex& ALineDirection,
                                  const TCoordinate& ACoef)
{
  assert(!ALineDirection.isNull());
  CVertex center = orthoProjectVertexOnLine(AVertex,
                                            ALineVertex, ALineDirection);
  return center + ACoef*(AVertex-center);
}
//******************************************************************************
INLINE
CVertex CGeometry::planarHomothety(const CVertex& AVertex,
                                   const CVertex& APlaneVertex,
                                   const CVertex& APlaneNormal,
                                   const TCoordinate& ACoef)
{
  assert(!APlaneNormal.isNull());
  CVertex center = orthoProjectVertexOnPlane(AVertex,
                                             APlaneVertex, APlaneNormal);
  return center + ACoef*(AVertex-center);
}
//******************************************************************************
INLINE
TCoordinate CGeometry::distanceToVertex(const CVertex& AVertex,
                                   const CVertex& AOtherVertex)
{
  return (AVertex-AOtherVertex).norm();
}
//******************************************************************************
INLINE
TCoordinate CGeometry::distanceToLine(const CVertex& AVertex,
                                 const CVertex& ALineVertex,
                                 const CVertex& ALineDirection)
{
  CVertex proj = orthoProjectVertexOnLine(AVertex, ALineVertex, ALineDirection);
  return (AVertex-proj).norm();
}
//******************************************************************************
INLINE
TCoordinate CGeometry::distanceToPlane(const CVertex& AVertex,
                                  const CVertex& APlaneVertex,
                                  const CVertex& APlaneNormal)
{
  CVertex proj = orthoProjectVertexOnPlane(AVertex, APlaneVertex, APlaneNormal);
  return (AVertex-proj).norm();
}
//******************************************************************************
INLINE
CVertex CGeometry::getBissectrix(const CVertex& AVector1,
                                 const CVertex& AVector2,
                                 const CVertex& ANormal)
{
  assert(!AVector1.isNull());
  assert(!AVector2.isNull());

  CVertex V1 = AVector1 / AVector1.norm();
  CVertex V2 = AVector2 / AVector2.norm();

  if ((V1+V2).isNull())
    {
      if (ANormal.isNull())
        return getNormalVector(V1);
      else
        return V1*ANormal;
    }
  else
    {
      if (isPositive((AVector1*AVector2).dot(ANormal)))
        return V1+V2;
      else
        return -(V1+V2);
    }
}
//******************************************************************************
INLINE
CVertex CGeometry::getNormalVector(const CVertex& AVector)
{
  assert(!AVector.isNull());

  if (isZero(AVector.getX()))
    return AVector.norm()*OX;

  // if (isZero(AVector.getY())) return OY;
  // if (isZero(AVector.getZ())) return OZ;

  CVertex V(-AVector.getY(), AVector.getX(), 0);

  return (AVector.norm()/V.norm()) * V;
}
//******************************************************************************
INLINE
CVertex CGeometry::getNormalVector(const CVertex& AVector1,
                                   const CVertex& AVector2)
{
  TCoordinate n1 = AVector1.norm();
  TCoordinate n2 = AVector2.norm();

  CVertex V = AVector1*AVector2;

  if (V.isNull())
    return ORIGIN;

  assert(!isZero(n1));
  assert(!isZero(n2));

  return ((n1+n2)/(2*V.norm()))*V;
}
//******************************************************************************
INLINE
CVertex CGeometry::getNormalVector(const CVertex& AVertex1,
                                   const CVertex& AVertex2,
                                   const CVertex& AVertex3)
{ return getNormalVector(AVertex2-AVertex1, AVertex3-AVertex2); }
//******************************************************************************
INLINE
CVertex CGeometry::getLinesIntersection(const CVertex& ALine1Vertex1,
                                        const CVertex& ALine1Vertex2,
                                        const CVertex& ALine2Vertex1,
                                        const CVertex& ALine2Vertex2)
{
  assert(ALine1Vertex1 != ALine1Vertex2);
  assert(ALine2Vertex1 != ALine2Vertex2);

#define A (ALine1Vertex1)
#define B (ALine1Vertex2)
#define C (ALine2Vertex1)
#define D (ALine2Vertex2)

  CVertex AB = B - A;
  CVertex CD = D - C;
  CVertex AC = C - A;

  CVertex ABvCD = AB*CD;

  TCoordinate t = det(AC,CD,ABvCD) / ABvCD.sqrNorm();

  return A + t*AB;

#undef A
#undef B
#undef C
#undef D
}
//******************************************************************************
INLINE
bool CGeometry::areColinear(const CVertex& AVector1,
                            const CVertex& AVector2)
{
  return (AVector1*AVector2).isNull();
}
//******************************************************************************
INLINE
TCoordinate CGeometry::getAngle(const CVertex& AVector1,
                                const CVertex& AVector2,
                                const CVertex& ANormal)
{
  // Formule:  V1.V2 = Norm(V1) * Norm(V2) * Cos ( Angle(V1,V2) )

  CVertex v1 = AVector1;
  CVertex v2 = AVector2;
  CVertex n  = ANormal;

  if (!v1.isNull()) v1 /= v1.norm();
  if (!v2.isNull()) v2 /= v2.norm();
  if (!n .isNull()) n  /= n .norm();

  TCoordinate cosinus = v1.dot(v2);

  if (cosinus > +1) cosinus = +1;
  if (cosinus < -1) cosinus = -1;

  TCoordinate angle = dAcos(cosinus);

  return isPositive((v1*v2).dot(n)) ? +angle : -angle;
}
//******************************************************************************
INLINE
CVertex CGeometry::getAngles(const CVertex& AVx,
                             const CVertex& AVz)
{
  if (AVx.isNull() || AVz.isNull())
    return ORIGIN;

  CTransformationMatrix matrix;

  CVertex Vx(orthoProjectVertexOnPlane(AVx, ORIGIN, AVz));
  CVertex Vy(AVz*Vx);
  CVertex Vz(AVz);

  TCoordinate alpha,beta,gamma;

  // gamma (pour obtenir: Vy.getX()==0 && Vy.getY()>0)
  // Après rotation, Vy doit se retrouver sur le vecteur N.
  // N est tel que: N.Vz==0 et N.getX()==0,
  // donc N = Vertex(0,Vz.getZ(),-Vz.getY())

  gamma = getAngle(CVertex(0,+Vz.getZ(),-Vz.getY()),Vy, Vz);

  matrix.rotate(Vz, -gamma);
  matrix.applyOn(Vx);
  matrix.applyOn(Vy);

  // beta (pour obtenir: Vx.getZ()==0)

  beta = getAngle(OX,Vx, Vy);

  matrix.setToIdentity();
  matrix.rotate(Vy, -beta);
  matrix.applyOn(Vx);
  matrix.applyOn(Vz);

  // alpha (pour obtenir: Vz.getY()==0)

  alpha = getAngle(OZ,Vz, Vx);

  return CVertex(alpha,beta,gamma);
}
//******************************************************************************
INLINE
int CGeometry::localiseVertexComparedToLine(const CVertex& AVertex,
                                            const CVertex& ALineVertex,
                                            const CVertex& ALineDirection,
                                            const CVertex& APlaneNormal)
{
  assert(!ALineDirection.isNull());
  assert(!APlaneNormal.isNull());

  CVertex direction = ALineDirection / ALineDirection.norm();
  CVertex normal    = APlaneNormal   / APlaneNormal  .norm();

  return sign((direction * normal).dot(AVertex-ALineVertex));
}
//******************************************************************************
INLINE
void CGeometry::getSegmentsIntersection(const CVertex& AVertexA,
                                        const CVertex& AVertexB,
                                        const CVertex& AVertexC,
                                        const CVertex& AVertexD,
                                        const CVertex& APlaneNormal,
                                        int& AIntersection1On,
                                        CVertex& AIntersection1,
                                        int& AIntersection2On,
                                        CVertex& AIntersection2)
{
  assert(!APlaneNormal.isNull());

  bool nullLength1 = AVertexA == AVertexB;
  bool nullLength2 = AVertexC == AVertexD;

  // Cas où un des deux segments au moins est de longueur nulle:
  if (nullLength1 || nullLength2)
    {
      // Les deux segments sont de longueur nulle, pas d'intersection:
      if (nullLength1 && nullLength2)
        {
          AIntersection1On = AIntersection2On = 0;
          return;
        }

      // Un seul segment est de longueur nulle.
      // Il y alpha au plus un sommet à insérer:
      AIntersection1On = 0;
      AIntersection2On = 0;

      return;
    }

  // Les deux segments ne sont pas de longueur nulle:
  CVertex directionAB = AVertexB - AVertexA;
  CVertex directionCD = AVertexD - AVertexC;

  int A = localiseVertexComparedToLine(AVertexA,
                                       AVertexC, directionCD,
                                       APlaneNormal);

  int B = localiseVertexComparedToLine(AVertexB,
                                       AVertexC, directionCD,
                                       APlaneNormal);

  if (A==0 && B==0)
    {
      // Les deux arêtes sont alignées (co-raffinement 1d):
      // Tri de A, B, C et D:
      const CVertex * vertices[4] =
      { & AVertexA, & AVertexB, & AVertexC, & AVertexD };

      CVertex normal = directionAB * APlaneNormal;

      assert(!normal.isNull());
      normal /= normal.norm();

      int i,j;

      // Tri à bulle, guère moins efficace qu'un quick-sort vu la taille du
      // tableau (4 éléments):
      for (i=0; i<3; ++i)
        for (j=i+1; j<4; ++j)
          if (localiseVertexComparedToLine(*vertices[i], *vertices[j],
                                           normal, APlaneNormal) == 1)
            {
              const CVertex * tmp = vertices[i];
              vertices[i] = vertices[j];
              vertices[j] = tmp;
            }

      bool firstSegment[4];

      for (i=0; i<4; ++i)
        firstSegment[i] = vertices[i]==&AVertexA || vertices[i]==&AVertexB;

      // Intersections:
      if (firstSegment[0]==firstSegment[1])
        {
          // Les segments sont disjoints:
          AIntersection1On = AIntersection2On = 0;
        }
      else
        {
          // Les segments se chevauchent, ou l'un est compris dans l'autre:
          if (*vertices[0] == *vertices[1])
            AIntersection1On = 0;
          else
            {
              AIntersection1On = firstSegment[0] ? 1 : 2;
              AIntersection1   = * vertices[1];
            }
        
          if (*vertices[2] == *vertices[3])
            AIntersection2On = 0;
          else
            {
              AIntersection2On = firstSegment[3] ? 1 : 2;
              AIntersection2   = * vertices[2];
            }
        }
    }
  else
    {
      // Les deux arêtes sont disjointes ou se croisent en un point unique:
      int C = localiseVertexComparedToLine(AVertexC,
                                           AVertexA, directionAB,
                                           APlaneNormal);

      int D = localiseVertexComparedToLine(AVertexD,
                                           AVertexA, directionAB,
                                           APlaneNormal);

      // assert(C!=0 || D!=0);

      int combineAB = combineSigns(A,B);
      int combineCD = combineSigns(C,D);

      AIntersection1On = (combineAB==-1 && combineCD<=0) ? 1 : 0;
      AIntersection2On = (combineCD==-1 && combineAB<=0) ? 2 : 0;

      if (AIntersection1On!=0 || AIntersection2On!=0)
        {
          AIntersection1 = AIntersection2 =
            getLinesIntersection(AVertexA, AVertexB, AVertexC, AVertexD);
        }
    }
}
//******************************************************************************
INLINE
void CGeometry::getSegmentsIntersection2d(const CVertex& AVertexA,
                                          const CVertex& AVertexB,
                                          const CVertex& AVertexC,
                                          const CVertex& AVertexD,
                                          int& AIntersection1On,
                                          CVertex& AIntersection1,
                                          int& AIntersection2On,
                                          CVertex& AIntersection2)
{
  getSegmentsIntersection(AVertexA, AVertexB, AVertexC, AVertexD,
                          OZ,
                          AIntersection1On, AIntersection1,
                          AIntersection2On, AIntersection2);
}
//******************************************************************************
INLINE
CVertex CGeometry::getRoundingNormalVector(const CVertex& AVertex1, 
                                           const CVertex& AVertex2,
                                           const CVertex& AVertex3)
{
#define A (AVertex1)
#define B (AVertex2)
#define C (AVertex3)
  
  CVertex AB = B - A;
  CVertex AC = C - A;

  if (CGeometry::areColinear(AB,AC))
    return A;
  else
    return AC * (AB * AC);

#undef A
#undef B
#undef C
}
//******************************************************************************

---