diff --git a/Code/Common/itkTriangleCell.h b/Code/Common/itkTriangleCell.h index 1238fa0fd08..835af8cfe43 100644 --- a/Code/Common/itkTriangleCell.h +++ b/Code/Common/itkTriangleCell.h @@ -87,6 +87,13 @@ class ITK_EXPORT TriangleCell: public TCellInterface, private TriangleCellTopolo virtual bool GetVertex(CellFeatureIdentifier, VertexAutoPointer &); virtual bool GetEdge(CellFeatureIdentifier, EdgeAutoPointer &); + virtual bool EvaluatePosition(CoordRepType [PointDimension], + PointsContainer* , + CoordRepType [PointDimension], + CoordRepType [], + double *, + InterpolationWeightType*); + /** Cell visitor interface. */ itkCellVisitMacro(TRIANGLE_CELL); @@ -101,6 +108,9 @@ class ITK_EXPORT TriangleCell: public TCellInterface, private TriangleCellTopolo private: TriangleCell(const Self&); //purposely not implemented void operator=(const Self&); //purposely not implemented + + double DistanceToLine(PointType x, PointType p1, PointType p2, + double &t, PointType closestPoint); }; diff --git a/Code/Common/itkTriangleCell.txx b/Code/Common/itkTriangleCell.txx index 5056098613a..cf305098c90 100644 --- a/Code/Common/itkTriangleCell.txx +++ b/Code/Common/itkTriangleCell.txx @@ -17,6 +17,7 @@ #ifndef _itkTriangleCell_txx #define _itkTriangleCell_txx #include "itkTriangleCell.h" +#include "vnl/algo/vnl_determinant.h" namespace itk { @@ -301,6 +302,377 @@ TriangleCell< TCellInterface > return true; } + + +/** Compute distance to finite line. Returns parametric coordinate t + * and point location on line. */ +template +double +TriangleCell< TCellInterface > +::DistanceToLine(PointType x, PointType p1, PointType p2, + double &t, PointType closestPoint) +{ + double denom, num; + PointType p21; + PointType closest; + double tolerance; + // + // Determine appropriate vectors + // + int i; + for(i=0;i 1.0 ) + { + closest = p2; + } + else + { + closest = p21; + for(i=0;i +bool +TriangleCell< TCellInterface > +::EvaluatePosition(CoordRepType x[PointDimension], + PointsContainer* points, + CoordRepType closestPoint[PointDimension], + CoordRepType pcoord[3], + double* minDist2, + InterpolationWeightType* weights) +{ + + if(PointDimension != 3) + { + itkWarningMacro("TriangleCell::EvaluatePosition() only works with 3D points"); + std::cout << "TriangleCell::EvaluatePosition() only works with 3D points" << std::endl; + return false; + } + + + int i, j; + double fabsn; + double rhs[2], c1[2], c2[2], n[3]; + double det; + double maxComponent; + int idx=0, indices[2]; + double dist2Point, dist2Line1, dist2Line2; + PointType closest; + PointType closestPoint1, closestPoint2, cp; + CoordRepType pcoords[3]; + + if(!points) + { + return false; + } + + // Get normal for triangle, only the normal direction is needed, i.e. the + // normal need not be normalized (unit length) + // + PointType pt1 = points->GetElement(m_PointIds[0]); + PointType pt2 = points->GetElement(m_PointIds[1]); + PointType pt3 = points->GetElement(m_PointIds[2]); + + + // This is the solution for 3D points + double ax, ay, az, bx, by, bz; + + // order is important!!! maintain consistency with triangle vertex order + ax = pt3[0] - pt2[0]; ay = pt3[1] - pt2[1]; az = pt3[2] - pt2[2]; + bx = pt1[0] - pt2[0]; by = pt1[1] - pt2[1]; bz = pt1[2] - pt2[2]; + + n[0] = (ay * bz - az * by); + n[1] = (az * bx - ax * bz); + n[2] = (ax * by - ay * bx); + + // Project point to plane + double t, n2; + PointType xo; + + for(i=0;i maxComponent) + { + maxComponent = fabsn; + idx = i; + } + } + + for (j=0, i=0; i<3; i++) + { + if ( i != idx ) + { + indices[j++] = i; + } + } + + for (i=0; i<2; i++) + { + rhs[i] = cp[indices[i]] - pt3[indices[i]]; + c1[i] = pt1[indices[i]] - pt3[indices[i]]; + c2[i] = pt2[indices[i]] - pt3[indices[i]]; + } + + + if ( (det = c1[0]*c2[1] - c2[0]*c1[1]) == 0.0 ) + { + pcoords[0] = pcoords[1] = pcoords[2] = 0.0; + if(pcoord) + { + pcoord[0] = pcoords[0]; + pcoord[1] = pcoords[1]; + pcoord[2] = pcoords[2]; + } + return false; + } + + pcoords[0] = (rhs[0]*c2[1] - c2[0]*rhs[1]) / det; + pcoords[1] = (c1[0]*rhs[1] - rhs[0]*c1[1]) / det; + pcoords[2] = 1.0 - (pcoords[0] + pcoords[1]); + + // Okay, now find closest point to element + // + if(weights) + { + weights[0] = pcoords[2]; + weights[1] = pcoords[0]; + weights[2] = pcoords[1]; + } + + if ( pcoords[0] >= 0.0 && pcoords[0] <= 1.0 && + pcoords[1] >= 0.0 && pcoords[1] <= 1.0 && + pcoords[2] >= 0.0 && pcoords[2] <= 1.0 ) + { + //projection distance + if (closestPoint) + { // Compute the Distance 2 Between Points + *minDist2 = 0; + for(i=0;iDistanceToLine(x,pt1,pt3,t,closestPoint1); + dist2Line2 = this->DistanceToLine(x,pt3,pt2,t,closestPoint2); + if (dist2Point < dist2Line1) + { + *minDist2 = dist2Point; + closest = pt3; + } + else + { + *minDist2 = dist2Line1; + closest = closestPoint1; + } + if (dist2Line2 < *minDist2) + { + *minDist2 = dist2Line2; + closest = closestPoint2; + } + for (i=0; i<3; i++) + { + closestPoint[i] = closest[i]; + } + } + else if ( pcoords[1] < 0.0 && pcoords[2] < 0.0 ) + { + dist2Point = 0; + for(i=0;iDistanceToLine(x,pt1,pt3,t,closestPoint1); + dist2Line2 = this->DistanceToLine(x,pt1,pt2,t,closestPoint2); + if (dist2Point < dist2Line1) + { + *minDist2 = dist2Point; + closest = pt1; + } + else + { + *minDist2 = dist2Line1; + closest = closestPoint1; + } + if (dist2Line2 < *minDist2) + { + *minDist2 = dist2Line2; + closest = closestPoint2; + } + for (i=0; i<3; i++) + { + closestPoint[i] = closest[i]; + } + } + else if ( pcoords[0] < 0.0 && pcoords[2] < 0.0 ) + { + dist2Point = 0; + for(i=0;iDistanceToLine(x,pt2,pt3,t,closestPoint1); + dist2Line2 = this->DistanceToLine(x,pt1,pt2,t,closestPoint2); + if (dist2Point < dist2Line1) + { + *minDist2 = dist2Point; + closest = pt2; + } + else + { + *minDist2 = dist2Line1; + closest = closestPoint1; + } + if (dist2Line2 < *minDist2) + { + *minDist2 = dist2Line2; + closest = closestPoint2; + } + for (i=0; i<3; i++) + { + closestPoint[i] = closest[i]; + } + } + else if ( pcoords[0] < 0.0 ) + { + *minDist2 = this->DistanceToLine(x,pt2,pt3,t,closestPoint); + } + else if ( pcoords[1] < 0.0 ) + { + *minDist2 = this->DistanceToLine(x,pt1,pt3,t,closestPoint); + } + else if ( pcoords[2] < 0.0 ) + { + *minDist2 = this->DistanceToLine(x,pt1,pt2,t,closestPoint); + } + } + if(pcoord) + { + pcoord[0] = pcoords[0]; + pcoord[1] = pcoords[1]; + pcoord[2] = pcoords[2]; + } + return false; + } +} + + } // end namespace itk #endif