C++ Simplex::getSize方法代码示例

 //! return true if p is in the simplex s
 static bool isInside(const Point<N> &p, Simplex<N> &s) {
   bool inside = true;
   std::vector<Point<N> > opposed;
   float m1[N][N]; // x par rapport a l'arrete
   float m2[N][N]; // R par rapport a l'arrete
   for (int k = 0; k < s.getSize(); k++) {
     opposed.clear();
     for (int i = 0; i < s.getSize(); i++) {
       if (i != k)
         opposed.push_back(s[i]);
     }
     // creation of matrix N*N
     for (size_t i = 0; i < N; i++) {
       for (size_t j = 0; j < N; j++) {
         m1[i][j] = p[i] - (opposed[j])[i];      // x par rapport a Fbi
         m2[i][j] = (s[k])[i] - (opposed[j])[i]; // R par rapport a Fbi
       }
     }
     if (Simplex<N>::determinant(m1, N) * Simplex<N>::determinant(m2, N) <
         0) { // determinants de signes different
       inside = false;
       break;
     }
   }
   return inside;
 }

	/**
	 * Update the simplex and return closest point to origin on the simplex
	 * @return Closest point to origin on the simplex
	 */
	template<class T> void GJKAlgorithm<T>::updateSimplex(Simplex<T> &simplex) const
	{
		Point3<T> closestPoint(0.0, 0.0, 0.0);
		T barycentrics[4];

		if(simplex.getSize() == 2)
		{ //simplex is a line (1D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1); //pointB is the last point added to the simplex

			closestPoint = LineSegment3D<T>(pointA, pointB).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
		}else if(simplex.getSize() == 3)
		{ //simplex is a triangle (2D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1);
			const Point3<T> &pointC = simplex.getPoint(2); //pointC is the last point added to the simplex

			const Vector3<T> co = pointC.vector(Point3<T>(0.0, 0.0, 0.0));
			const Vector3<T> cb = pointC.vector(pointB);
			const Vector3<T> ca = pointC.vector(pointA);
			const Vector3<T> normalAbc = cb.crossProduct(ca);

			closestPoint = Triangle3D<T>(pointA, pointB, pointC).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
			simplex.setBarycentric(2, barycentrics[2]);

			if(barycentrics[1]==0.0)
			{ //remove pointB
				simplex.removePoint(1);
			}
			if(barycentrics[0]==0.0)
			{ //remove pointA
				simplex.removePoint(0);
			}

			if(normalAbc.dotProduct(co) <= 0.0)
			{ //voronoi region -ABC => ABC
				simplex.swapPoints(0, 1); //swap pointA and pointB
			}
		}else if (simplex.getSize() == 4)
		{ //simplex is a tetrahedron (3D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1);
			const Point3<T> &pointC = simplex.getPoint(2);
			const Point3<T> &pointD = simplex.getPoint(3); //pointD is the last point added to the simplex

			const short voronoiRegionMask = 14; //test all voronoi regions except the one which doesn't include the new point added (pointD)
			closestPoint = Tetrahedron<T>(pointA, pointB, pointC, pointD).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics, voronoiRegionMask);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
			simplex.setBarycentric(2, barycentrics[2]);
			simplex.setBarycentric(3, barycentrics[3]);

			if(barycentrics[2]==0.0)
			{ //remove pointC
				simplex.removePoint(2);
			}
			if(barycentrics[1]==0.0)
			{ //remove pointB
				simplex.removePoint(1);
			}
			if(barycentrics[0]==0.0)
			{ //remove pointA
				simplex.removePoint(0);
			}
		}else
		{
			std::ostringstream oss;
			oss << simplex.getSize();
			throw std::invalid_argument("Size of simplex unsupported: " + oss.str() + ".");
		}

		simplex.setClosestPointToOrigin(closestPoint);
	}