text: /*=========================================================================
   *
   *  Copyright NumFOCUS
   *
   *  Licensed under the Apache License, Version 2.0 (the "License");
   *  you may not use this file except in compliance with the License.
   *  You may obtain a copy of the License at
   *
   *         https://www.apache.org/licenses/LICENSE-2.0.txt
   *
   *  Unless required by applicable law or agreed to in writing, software
   *  distributed under the License is distributed on an "AS IS" BASIS,
   *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   *  See the License for the specific language governing permissions and
   *  limitations under the License.
   *
   *=========================================================================*/
  #ifndef itkTriangleCell_hxx
  #define itkTriangleCell_hxx
  #include "vnl/algo/vnl_determinant.h"
  
  namespace itk
  {
  /**
   * Standard CellInterface:
   */
  template <typename TCellInterface>
  void
  TriangleCell<TCellInterface>::MakeCopy(CellAutoPointer & cellPointer) const
  {
    cellPointer.TakeOwnership(new Self);
    cellPointer->SetPointIds(this->GetPointIds());
  }
  
  /**
   * Standard CellInterface:
   * Get the topological dimension of this cell.
   */
  template <typename TCellInterface>
  unsigned int
  TriangleCell<TCellInterface>::GetDimension() const
  {
    return Self::CellDimension;
  }
  
  /**
   * Standard CellInterface:
   * Get the number of points required to define the cell.
   */
  template <typename TCellInterface>
  unsigned int
  TriangleCell<TCellInterface>::GetNumberOfPoints() const
  {
    return Self::NumberOfPoints;
  }
  
  /**
   * Standard CellInterface:
   * Get the number of boundary features of the given dimension.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::GetNumberOfBoundaryFeatures(int dimension) const -> CellFeatureCount
  {
    switch (dimension)
    {
      case 0:
        return GetNumberOfVertices();
      case 1:
        return GetNumberOfEdges();
      default:
        return 0;
    }
  }
  
  /**
   * Standard CellInterface:
   * Get the boundary feature of the given dimension specified by the given
   * cell feature Id.
   * The Id can range from 0 to GetNumberOfBoundaryFeatures(dimension)-1.
   */
  template <typename TCellInterface>
  bool
  TriangleCell<TCellInterface>::GetBoundaryFeature(int                   dimension,
                                                   CellFeatureIdentifier featureId,
                                                   CellAutoPointer &     cellPointer)
  {
    switch (dimension)
    {
      case 0:
      {
        VertexAutoPointer vertexPointer;
        if (this->GetVertex(featureId, vertexPointer))
        {
          TransferAutoPointer(cellPointer, vertexPointer);
          return true;
        }
        break;
      }
      case 1:
      {
        EdgeAutoPointer edgePointer;
        if (this->GetEdge(featureId, edgePointer))
        {
          TransferAutoPointer(cellPointer, edgePointer);
          return true;
        }
        break;
      }
      default:
        break; // just fall through and return false
    }
    cellPointer.Reset();
    return false;
  }
  
  /**
   * Standard CellInterface:
   * Set the point id list used by the cell.  It is assumed that the given
   * iterator can be incremented and safely de-referenced enough times to
   * get all the point ids needed by the cell.
   */
  template <typename TCellInterface>
  void
  TriangleCell<TCellInterface>::SetPointIds(PointIdConstIterator first)
  {
    PointIdConstIterator ii(first);
  
    for (unsigned int i = 0; i < NumberOfPoints; ++i, ++ii)
    {
      m_PointIds[i] = *ii;
    }
  }
  
  /**
   * Standard CellInterface:
   * Set the point id list used by the cell.  It is assumed that the range
   * of iterators [first, last) contains the correct number of points needed to
   * define the cell.  The position *last is NOT referenced, so it can safely
   * be one beyond the end of an array or other container.
   */
  template <typename TCellInterface>
  void
  TriangleCell<TCellInterface>::SetPointIds(PointIdConstIterator first, PointIdConstIterator last)
  {
    unsigned int         localId = 0;
    PointIdConstIterator ii(first);
  
    while ((ii != last) && (localId < NumberOfPoints))
    {
      m_PointIds[localId++] = *ii++;
    }
  }
  
  /**
   * Standard CellInterface:
   * Set an individual point identifier in the cell.
   */
  template <typename TCellInterface>
  void
  TriangleCell<TCellInterface>::SetPointId(int localId, PointIdentifier ptId)
  {
    m_PointIds[localId] = ptId;
  }
  
  /**
   * Standard CellInterface:
   * Get a begin iterator to the list of point identifiers used by the cell.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::PointIdsBegin() -> PointIdIterator
  {
    return &m_PointIds[0];
  }
  
  /**
   * Standard CellInterface:
   * Get a const begin iterator to the list of point identifiers used
   * by the cell.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::PointIdsBegin() const -> PointIdConstIterator
  {
    return &m_PointIds[0];
  }
  
  /**
   * Standard CellInterface:
   * Get an end iterator to the list of point identifiers used by the cell.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::PointIdsEnd() -> PointIdIterator
  {
    return &m_PointIds[Self::NumberOfPoints - 1] + 1;
  }
  
  /**
   * Standard CellInterface:
   * Get a const end iterator to the list of point identifiers used
   * by the cell.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::PointIdsEnd() const -> PointIdConstIterator
  {
    return &m_PointIds[Self::NumberOfPoints - 1] + 1;
  }
  
  /**
   * Triangle-specific:
   * Get the number of vertices defining the triangle.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::GetNumberOfVertices() const -> CellFeatureCount
  {
    return Self::NumberOfVertices;
  }
  
  /**
   * Triangle-specific:
   * Get the number of edges defined for the triangle.
   */
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::GetNumberOfEdges() const -> CellFeatureCount
  {
    return Self::NumberOfEdges;
  }
  
  /**
   * Triangle-specific:
   * Get the vertex specified by the given cell feature Id.
   * The Id can range from 0 to GetNumberOfVertices()-1.
   */
  template <typename TCellInterface>
  bool
  TriangleCell<TCellInterface>::GetVertex(CellFeatureIdentifier vertexId, VertexAutoPointer & vertexPointer)
  {
    auto * vert = new VertexType;
  
    vert->SetPointId(0, m_PointIds[vertexId]);
    vertexPointer.TakeOwnership(vert);
    return true;
  }
  
  /**
   * Triangle-specific:
   * Get the edge specified by the given cell feature Id.
   * The Id can range from 0 to GetNumberOfEdges()-1.
   */
  template <typename TCellInterface>
  bool
  TriangleCell<TCellInterface>::GetEdge(CellFeatureIdentifier edgeId, EdgeAutoPointer & edgePointer)
  {
    auto * edge = new EdgeType;
  
    for (unsigned int i = 0; i < EdgeType::NumberOfPoints; ++i)
    {
      edge->SetPointId(i, m_PointIds[m_Edges[edgeId][i]]);
    }
    edgePointer.TakeOwnership(edge);
    return true;
  }
  
  /** Compute distance to finite line. Returns parametric coordinate t
   *  and point location on line. */
  template <typename TCellInterface>
  double
  TriangleCell<TCellInterface>::DistanceToLine(PointType      x,
                                               PointType      p1,
                                               PointType      p2,
                                               double &       t,
                                               CoordRepType * closestPoint)
  {
    // convert from CoordRepType * to PointType:
    PointType temp(closestPoint);
    //   for (unsigned int i = 0; i < PointDimension; ++i)
    //     {
    //     temp[i] = closestPoint[i];
    //     }
  
    // Compute the squared distance to the line:
    const double distance2 = this->DistanceToLine(x, p1, p2, t, temp);
  
    // convert from PointType to CoordRepType * :
    for (unsigned int j = 0; j < PointDimension; ++j)
    {
      closestPoint[j] = temp[j];
    }
  
    return distance2;
  }
  
  template <typename TCellInterface>
  double
  TriangleCell<TCellInterface>::DistanceToLine(PointType   x,
                                               PointType   p1,
                                               PointType   p2,
                                               double &    t,
                                               PointType & closestPoint)
  {
    VectorType v21 = p2 - p1;
    //
    //   Get parametric location
    //
    double num(0);
    double denom(0);
  
    for (unsigned int i = 0; i < PointDimension; ++i)
    {
      num += static_cast<double>(v21[i] * (x[i] - p1[i]));
      denom += static_cast<double>(v21[i] * v21[i]);
    }
  
    // trying to avoid an expensive fabs
    double tolerance = 1.e-05 * num;
    if (tolerance < 0.0)
    {
      tolerance = -tolerance;
    }
    if ((-tolerance < denom) && (denom < tolerance)) // numerically bad!
    {
      closestPoint = p1; // arbitrary, point is (numerically) far away
    }
    //
    // If parametric coordinate is within 0<=p<=1, then the point is closest to
    // the line.  Otherwise, it's closest to a point at the end of the line.
    //
    else if ((t = num / denom) < 0.0)
    {
      closestPoint = p1;
    }
    else if (t > 1.0)
    {
      closestPoint = p2;
    }
    else
    {
      closestPoint = p1 + v21 * t;
    }
  
    return static_cast<double>(closestPoint.SquaredEuclideanDistanceTo(x));
  }
  
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::ComputeArea(PointsContainer * iPoints) -> CoordRepType
  {
    PointType p[3];
  
    for (unsigned int i = 0; i < NumberOfPoints; ++i)
    {
      p[i] = iPoints->GetElement(m_PointIds[i]);
    }
  
    CoordRepType a = p[1].EuclideanDistanceTo(p[2]);
    CoordRepType b = p[0].EuclideanDistanceTo(p[2]);
    CoordRepType c = p[1].EuclideanDistanceTo(p[0]);
  
    CoordRepType s = 0.5 * (a + b + c);
    return std::sqrt(s * (s - a) * (s - b) * (s - c));
  }
  
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::ComputeBarycenter(CoordRepType * iWeights, PointsContainer * iPoints) -> PointType
  {
    PointType    p[3];
    CoordRepType sum_weights(0.);
    unsigned int i(0);
  
    for (; i < 3; ++i)
    {
      sum_weights += iWeights[i];
      p[i] = iPoints->GetElement(m_PointIds[i]);
    }
  
    PointType oP;
  
    if (sum_weights != 0.)
    {
      oP.Fill(0.);
      for (i = 0; i < 3; ++i)
      {
        oP += p[i].GetVectorFromOrigin() * iWeights[i] / sum_weights;
      }
    }
    else
    {
      oP = p[0];
    }
    return oP;
  }
  
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::ComputeCenterOfGravity(PointsContainer * iPoints) -> PointType
  {
    std::vector<CoordRepType> weights(3, 1. / 3.);
    return ComputeBarycenter(&weights[0], iPoints);
  }
  
  template <typename TCellInterface>
  auto
  TriangleCell<TCellInterface>::ComputeCircumCenter(PointsContainer * iPoints) -> PointType
  {
    std::vector<CoordRepType> weights(3, 0.);
  
    PointType    p[3];
    unsigned int i;
  
    for (i = 0; i < 3; ++i)
    {
      p[i] = iPoints->GetElement(m_PointIds[i]);
    }
  
    CoordRepType a = p[1].SquaredEuclideanDistanceTo(p[2]);
    CoordRepType b = p[0].SquaredEuclideanDistanceTo(p[2]);
    CoordRepType c = p[1].SquaredEuclideanDistanceTo(p[0]);
  
    weights[0] = a * (b + c - a);
    weights[1] = b * (c + a - b);
    weights[2] = c * (a + b - c);
  
    CoordRepType sum_weights = weights[0] + weights[1] + weights[2];
  
    if (sum_weights != 0.)
    {
      PointType oP;
      oP.Fill(0.);
  
      for (i = 0; i < 3; ++i)
      {
        oP += p[i].GetVectorFromOrigin() * weights[i] / sum_weights;
      }
  
      return oP;
    }
    else
    {
      return p[0];
    }
  }
  
  /** Evaluate the position of a given point inside the cell */
  template <typename TCellInterface>
  bool
  TriangleCell<TCellInterface>::EvaluatePosition(CoordRepType *            x,
                                                 PointsContainer *         points,
                                                 CoordRepType *            closestPoint,
                                                 CoordRepType              pcoord[3],
                                                 double *                  minDist2,
                                                 InterpolationWeightType * weights)
  {
    unsigned int i;
    double       dist2Point;
    double       dist2Line1;
    double       dist2Line2;
    PointType    closest;
    PointType    closestPoint1;
    PointType    closestPoint2;
    PointType    X(x);
  
    if (!points)
    {
      return false;
    }
  
    //
    // Get the vertexes of this triangle
    //
    PointType pt1 = points->GetElement(m_PointIds[0]);
    PointType pt2 = points->GetElement(m_PointIds[1]);
    PointType pt3 = points->GetElement(m_PointIds[2]);
  
    //
    // Compute Vectors along the edges.
    // These two vectors form a vector base for the 2D space of the triangle cell.
    //
    VectorType v12 = pt1 - pt2;
    VectorType v32 = pt3 - pt2;
  
    //
    // Compute Vectors in the dual vector base inside the 2D space of the triangle
    // cell.
    // u12 is orthogonal to v32
    // u32 is orthogonal to v12
    //
    const double dotproduct = v12 * v32;
    VectorType   u12 = v12 - v32 * (dotproduct / v32.GetSquaredNorm());
    VectorType   u32 = v32 - v12 * (dotproduct / v12.GetSquaredNorm());
  
    //
    // Add normalizations for making {u12,u32} a vector basis orthonormal to {v12,
    // v32}.
    //
    u12 /= (u12 * v12);
    u32 /= (u32 * v32);
  
    //
    // Project point to plane, by using the dual vector base
    //
    // Compute components of the input point in the 2D
    // space defined by v12 and v32
    //
    VectorType xo = X - pt2;
  
    const double u12p = xo * u12;
    const double u32p = xo * u32;
  
    VectorType x12 = v12 * u12p;
    VectorType x32 = v32 * u32p;
  
    //
    // The projection of point X in the plane is cp
    //
    PointType cp = pt2 + x12 + x32;
  
    //
    // Compute barycentric coordinates in the Triangle
    //
    const double b1 = u12p;
    const double b2 = 1.0 - u12p - u32p;
    const double b3 = u32p;
  
    //
    // Test if the projected point is inside the cell.
    //
    // Zero with epsilon
    const double zwe = -NumericTraits<double>::min();
  
    //
    // Since the three barycentric coordinates are interdependent
    // only three tests should be necessary. That is, we only need
    // to test against the equations of three lines (half-spaces).
    //
    if ((b1 >= zwe) && (b2 >= zwe) && (b3 >= zwe))
    {
      //
      // This is the case when the point is inside the triangle
      // projection distance
      if (closestPoint)
      { // Compute the Distance 2 Between Points
        *minDist2 = 0;
        for (i = 0; i < PointDimension; ++i)
        {
          const double val = cp[i] - x[i];
          *minDist2 += val * val;
          closestPoint[i] = cp[i];
        }
      }
  
      if (pcoord)
      {
        pcoord[0] = b1;
        pcoord[1] = b2;
        pcoord[2] = b3;
      }
  
      if (weights)
      {
        weights[0] = b1;
        weights[1] = b2;
        weights[2] = b3;
      }
  
      return true;
    }
    else
    {
      if (closestPoint)
      {
        double lt; // parameter along the line (not used)
        if (b1 < 0.0 && b2 < 0.0)
        {
          dist2Point = 0;
          for (i = 0; i < PointDimension; ++i)
          {
            dist2Point += (x[i] - pt3[i]) * (x[i] - pt3[i]);
          }
          dist2Line1 = this->DistanceToLine(x, pt1, pt3, lt, closestPoint1);
          dist2Line2 = this->DistanceToLine(x, pt3, pt2, lt, closestPoint2);
          if (dist2Point < dist2Line1)
          {
            *minDist2 = dist2Point;
            closest = pt3;
          }
          else
          {
            *minDist2 = dist2Line1;
            closest = closestPoint1;
          }
          if (dist2Line2 < *minDist2)
          {
            *minDist2 = dist2Line2;
            closest = closestPoint2;
          }
          for (i = 0; i < PointDimension; ++i)
          {
            closestPoint[i] = closest[i];
          }
          for (; i < 3; ++i)
          {
            closestPoint[i] = 0.;
          }
        }
        else if (b2 < 0.0 && b3 < 0.0)
        {
          dist2Point = 0;
          for (i = 0; i < PointDimension; ++i)
          {
            dist2Point += (x[i] - pt1[i]) * (x[i] - pt1[i]);
          }
          dist2Line1 = this->DistanceToLine(x, pt1, pt3, lt, closestPoint1);
          dist2Line2 = this->DistanceToLine(x, pt1, pt2, lt, closestPoint2);
          if (dist2Point < dist2Line1)
          {
            *minDist2 = dist2Point;
            closest = pt1;
          }
          else
          {
            *minDist2 = dist2Line1;
            closest = closestPoint1;
          }
          if (dist2Line2 < *minDist2)
          {
            *minDist2 = dist2Line2;
            closest = closestPoint2;
          }
          for (i = 0; i < PointDimension; ++i)
          {
            closestPoint[i] = closest[i];
          }
          for (; i < 3; ++i)
          {
            closestPoint[i] = 0.;
          }
        }
        else if (b1 < 0.0 && b3 < 0.0)
        {
          dist2Point = 0;
          for (i = 0; i < PointDimension; ++i)
          {
            dist2Point += (x[i] - pt2[i]) * (x[i] - pt2[i]);
          }
          dist2Line1 = this->DistanceToLine(x, pt2, pt3, lt, closestPoint1);
          dist2Line2 = this->DistanceToLine(x, pt1, pt2, lt, closestPoint2);
          if (dist2Point < dist2Line1)
          {
            *minDist2 = dist2Point;
            closest = pt2;
          }
          else
          {
            *minDist2 = dist2Line1;
            closest = closestPoint1;
          }
          if (dist2Line2 < *minDist2)
          {
            *minDist2 = dist2Line2;
            closest = closestPoint2;
          }
          for (i = 0; i < PointDimension; ++i)
          {
            closestPoint[i] = closest[i];
          }
          for (; i < 3; ++i)
          {
            closestPoint[i] = 0.;
          }
        }
        else if (b1 < 0.0)
        {
          *minDist2 = this->DistanceToLine(x, pt2, pt3, lt, closestPoint);
        }
        else if (b2 < 0.0)
        {
          *minDist2 = this->DistanceToLine(x, pt1, pt3, lt, closestPoint);
        }
        else if (b3 < 0.0)
        {
          *minDist2 = this->DistanceToLine(x, pt1, pt2, lt, closestPoint);
        }
      }
      if (pcoord)
      {
        pcoord[0] = b1;
        pcoord[1] = b2;
        pcoord[2] = b3;
      }
      // Just fall through to default return false;
    }
    return false; // Default case that should never be reached.
  }
  } // end namespace itk
  
  #endif
  