C++ Simplex类代码示例

int main() {
  init();
  // read input
  cin >> n >> m;
  for (int i=0;i<m;i++) {
    for (int j=0;j<3;j++) cin >> e[i][j];
    nextE[e[i][0]].push_back(i);
    nextE[e[i][1]].push_back(i);
    next[e[i][0]].push_back(e[i][1]);
    next[e[i][1]].push_back(e[i][0]);
  }
  // set up linear program
  nA=0;
  for (int i=0;i<n;i++) cycles(i);
  for (int i=0;i<m;i++) {
    A[nA][2*i+1]=1; b[nA]=e[i][2];
    nA++;
    c[2*i]=-1; c[2*i+1]=1;
  }
  // solve
  Simplex s = Simplex(A, b, c, nA, 2*m);
  Fraction f = s.solve();
  f = f.neg();
  for (int i=0;i<m;i++) f = f.add(Fraction(e[i][2]));
  cout << f.str() << endl;
  return 0;
}

Point OjaData::oja_rank(const Point& x) const
{
	errif(x.dim() != dim(), "OjaData::oja_rank: Illegal dimension on point " << x);

	if (plane)
		return plane->oja_rank(x);

	Point Gr(dim());
	Simplex S;
	double sgn;
	double k = 0;

	for (Index I(dim(), size()); I; I++, k++)
	{
		S.get(*this, I, x);
		sgn = S.sign();
		for (int j = 0; j<dim(); j++)
			Gr[j] += S.row_cof(j + 1) * sgn;
	}

	for (int i = 0; i < dim(); i++)
		Gr[i] /= k;

	return Gr;
}

  //! Retourne la valeur interpolée d'un point dans le pavage
  float interpolateValue(const Point<N> &Q) {
    bool found = false;
    float value = 0;
    float rapport = 0;
    size_t iSimplex;
    for (iSimplex = 0; iSimplex < multiplex.size(); iSimplex++) {
      if (isInside(Q, multiplex[iSimplex])) {
        found = true;
        break;
      }
    }
    if (!found) {
      cout << "Le point n'est dans aucun simplex" << endl;
      return 0;
    }

    //  le rapport entre le volume du N-simplexe {Q,Fbj} et le volume du
    //  N-simplexe D={Pbj,Fbj}.
    for (int i = 0; i < multiplex[iSimplex].getSize(); i++) {
      Simplex<N> temp;
      temp.pushPoint(Q);
      for (int j = 0; j < multiplex[iSimplex].getSize(); j++) {
        if (i != j)
          temp.pushPoint((multiplex[iSimplex])[j]);
      }
      // temp le simplex {Q,Fbj}
      // <multiplex[iSimplex] le simplex {Pbj,Fbj}
      rapport = temp.calculateVol() / multiplex[iSimplex].calculateVol();
      value += rapport * (multiplex[iSimplex][i]).getValeur();
    }
    return value;
  }

 //! 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;
 }

void add_simplices(SimplexVector& sv, int d, const PointContainer& points)
{
	PointIndex indices[d+1];
	for (int i = 0; i < d+1; ++i) 
		indices[i] = d - i;

	while(indices[d] < points.size() - d)
	{
		// Add simplex
		Miniball mb(points[indices[0]].dim());
		Simplex s;
		for (int i = 0; i < d+1; ++i)
		{
			s.add(indices[i]);
			mb.check_in(points[indices[i]]);
		}
		mb.build();
		s.set_value(mb.squared_radius());
		sv.push_back(s);

		
		// Advance indices
		for (int i = 0; i < d+1; ++i)
		{
			++indices[i];
			if (indices[i] < points.size() - i)
			{
				for (int j = i-1; j >= 0; --j)
					indices[j] = indices[j+1] + 1;
				break;
			}
		}
	}
}

double Handler::optimise_single(vector<double> &parameters, vector<vector<double> > &results, vector<vector<vector<double> > > &allResults, int& itr){
  double SSE = 99999999999999.9;
  double tempSSE;
  vector<double> tempPar, seedParams;
  Simplex simplex;

  parameters.clear();
  allResults.clear();
  parameters = generate_seed_parameters();
  for(int index = 0;index<20;index++){
    seedParams.clear();
    seedParams = generate_seed_parameters();
    //printcon(seedParams);
    if(this->useMLE == false) tempPar = simplex.neldermead(&Handler::fitEpidemicsMLE, *this,  seedParams, itr);
    else tempPar = simplex.neldermead(&Handler::fitEpidemics, *this,  seedParams, itr);
    // Store the SSE value for this
    tempSSE = fitEpidemics(tempPar);
    if(tempSSE < SSE){
      SSE = tempSSE;
      parameters=tempPar;
    }
    cout << "." << flush;
  }
  results = ode_solve(parameters);
  allResults = ode_solve_separate(parameters);
  //allResults.clear();
  cout << endl;
  return SSE;
}

	void SetToSimplex(Simplex& s){
		data.clear();
		for(unsigned int i = 0; i< s.GetSize(); i++){
			data.push_back(s[i]);
			dataA.push_back(s.atA(i));
			dataB.push_back(s.atB(i));
		}
	}

 static void PrintSimplex(std::ostream &str, const Simplex &simplex)
 {
     for (typename Simplex::const_iterator i = simplex.begin(); i != simplex.end(); i++)
     {
         str<<(*i)<<" ";
     }
     str<<std::endl;
 }

int main()
{
	Simplex s;
	s.getInput();
	cout << "\n\n";
	s.simplexMethod();
	return 0;
}

/* Optimisation procedure. Takes a reference to parameters, combined results, component results and number of iterations taken. The function updates these
   arguments with the results of the Nelder-Mead optimisation procedure. */
double Handler::optimiseEpidemics(vector<double> &parameters, vector<vector<double> > &results, vector<vector<vector<double> > > &allResults, int& itr){
  double SSE = 99999999999999999.9;
  double tempSSE;
  vector<double> tempPar, seedParams;
  Simplex simplex;
  int iterations = 10001;

  // If no epidemics have yet been detected, use the mean of the current data as the 
  // current model and return the corresponding SSE.
  if(epidemics.size() == 0){
    //baseModel = base_model(temp_data);
    results = base_model(temp_data);
    SSE = calculate_SSE(results, temp_data);
    parameters.clear();
    allResults.clear();
    allResults.push_back(baseModel);
    return(SSE);
  }
  srand(time(NULL));
  parameters = generate_seed_parameters();

  // If there are epidemics to be fitted, perform X random fits and keep best fitting model
  for(int index=0;index<10;index++){
    // Create random seed parameters
    seedParams.clear();      
    seedParams = generate_seed_parameters();
    
    while(fitEpidemics(seedParams) != fitEpidemics(seedParams)){
      seedParams.clear();            
      seedParams = generate_seed_parameters();
    }
    
    // Get the optimised parameters from nelder mead algorithm
    tempPar = simplex.neldermead(&Handler::fitEpidemics, *this,  seedParams, iterations);
    // Store the SSE value for this
 
    tempSSE = fitEpidemics(tempPar);
    //if(tempSSE == tempSSE) cout << "Temp RSquare: " << (1-tempSSE/(SStot(temp_data,1))) << endl;
 
    // If this SSE value is better than the previous, store it and the corresponding parameters
    if(tempSSE < SSE){
      itr = iterations;
      SSE = tempSSE;
      parameters=tempPar;
    }
    cout << "." << flush;
  }

  // Get the combined values from these parameters, as well as a vector of 
  // each sub-epidemic
  results = ode_solve(parameters);
   
  allResults.clear();
  allResults = ode_solve_separate(parameters);
  cout << endl;
  return(SSE);

}

int main()
{
	struct user_data::_userdata ud;
	user_data::get_data_from_user(&ud);
	

	Simplex s;
	s.generate_plane(&ud);
	s.run();

	return 0;
}

	/**
	* @param includeMargin Indicate whether algorithm operates on objects with margin
	*/
	template<class T> std::unique_ptr<GJKResult<T>> GJKAlgorithm<T>::processGJK(const CollisionConvexObject3D &convexObject1,
			const CollisionConvexObject3D &convexObject2, bool includeMargin) const
	{
		//get point which belongs to the outline of the shape (Minkowski difference)
		Vector3<T> initialDirection = Vector3<T>(1.0, 0.0, 0.0);
		Point3<T> initialSupportPointA = convexObject1.getSupportPoint(initialDirection.template cast<float>(), includeMargin).template cast<T>();
		Point3<T> initialSupportPointB = convexObject2.getSupportPoint((-initialDirection).template cast<float>(), includeMargin).template cast<T>();
		Point3<T> initialPoint = initialSupportPointA - initialSupportPointB;

		Vector3<T> direction = (-initialPoint).toVector();

		Simplex<T> simplex;
		simplex.addPoint(initialSupportPointA, initialSupportPointB);

		T minimumToleranceMultiplicator = (T)1.0;

		for(unsigned int iterationNumber=0; iterationNumber<maxIteration; ++iterationNumber)
		{
			Point3<T> supportPointA = convexObject1.getSupportPoint(direction.template cast<float>(), includeMargin).template cast<T>();
			Point3<T> supportPointB = convexObject2.getSupportPoint((-direction).template cast<float>(), includeMargin).template cast<T>();
			Point3<T> newPoint = supportPointA - supportPointB;

			const Vector3<T> &vClosestPoint = -direction; //vector from origin to closest point of simplex
			T closestPointSquareDistance = vClosestPoint.dotProduct(vClosestPoint);
			T closestPointDotNewPoint = vClosestPoint.dotProduct(newPoint.toVector());

			//check termination conditions: new point is not more extreme that existing ones OR new point already exist in simplex
			T distanceTolerance = std::max(minimumTerminationTolerance*minimumToleranceMultiplicator, relativeTerminationTolerance*closestPointSquareDistance);
			if((closestPointSquareDistance-closestPointDotNewPoint) <= distanceTolerance || simplex.isPointInSimplex(newPoint))
			{
				if(closestPointDotNewPoint <= 0.0)
				{ //collision detected
					return std::make_unique<GJKResultCollide<T>>(simplex);
				}else
				{
					return std::make_unique<GJKResultNoCollide<T>>(std::sqrt(closestPointSquareDistance), simplex);
				}
			}

			simplex.addPoint(supportPointA, supportPointB);

			direction = (-simplex.getClosestPointToOrigin()).toVector();

			minimumToleranceMultiplicator += percentageIncreaseOfMinimumTolerance;
		}

		#ifdef _DEBUG
			logMaximumIterationReach();
		#endif

		return std::make_unique<GJKResultInvalid<T>>();
	}

D3DXVECTOR3 CollisionSolver::GetConvexHullPenetration(const CollisionMesh& particle, 
                                                      const CollisionMesh& hull, 
                                                      Simplex& simplex)
{
    D3DXVECTOR3 furthestPoint;
    D3DXVECTOR3 penetrationDirection;
    float penetrationDistance = 0.0f;
    bool penetrationFound = false;
    const float minDistance = 0.1f;
    const int maxIterations = 10;
    int iteration = 0;

    while(!penetrationFound && iteration < maxIterations)
    {
        ++iteration;
        const Face& face = simplex.GetClosestFaceToOrigin();
        penetrationDirection = face.normal;
        penetrationDistance = face.distanceToOrigin;
        penetrationFound = penetrationDistance == 0.0f;

        if(!penetrationFound)
        {
            // Check if there are any edge points beyond the closest face
            furthestPoint = GetMinkowskiSumEdgePoint(face.normal, particle, hull);
            const D3DXVECTOR3 faceToPoint = furthestPoint - simplex.GetPoint(face.indices[0]);
            const float distance = fabs(D3DXVec3Dot(&faceToPoint, &face.normal));
            penetrationFound = distance < minDistance;

            if(!penetrationFound)
            {
                // Add the new point and extend the convex hull
                simplex.ExtendFace(furthestPoint);
            }
        }
    }

    if(!penetrationFound)
    {
        // Fallback on the initial closest face
        const Face& face = simplex.GetClosestFaceToOrigin();
        penetrationDirection = face.normal;
        penetrationDistance = face.distanceToOrigin;
    }

    if(particle.RenderSolverDiagnostics())
    {
        UpdateDiagnostics(simplex, furthestPoint);
    }

    return -(penetrationDirection * penetrationDistance);
}

bool CollisionSolver::SolveTetrahedronSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
    const D3DXVECTOR3& pointA = simplex.GetPoint(3);
    const D3DXVECTOR3& pointB = simplex.GetPoint(0);
    const D3DXVECTOR3& pointC = simplex.GetPoint(1);
    const D3DXVECTOR3& pointD = simplex.GetPoint(2);

    const D3DXVECTOR3 AB = pointB - pointA;
    const D3DXVECTOR3 AC = pointC - pointA;
    const D3DXVECTOR3 AD = pointD - pointA;
    const D3DXVECTOR3 AO = -pointA;

    // Check if within the three surrounding planes
    // The forth plane has been previously tested with the plane simplex
    // All normals will point to the center of the tetrahedron
    D3DXVECTOR3 CBnormal, BDnormal, DCnormal;
    D3DXVec3Cross(&CBnormal, &AC, &AB);
    D3DXVec3Cross(&BDnormal, &AB, &AD);
    D3DXVec3Cross(&DCnormal, &AD, &AC);

    const float CBdistance = D3DXVec3Dot(&CBnormal, &AO);
    const float BDdistance = D3DXVec3Dot(&BDnormal, &AO);
    const float DCdistance = D3DXVec3Dot(&DCnormal, &AO);

    bool originInsideSimplex = true;
    if(CBdistance < 0.0f)
    {
        // Origin is outside of the CB plane
        // D is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointD);
        direction = -CBnormal;
        originInsideSimplex = false;
    }
    else if(BDdistance < 0.0f)
    {
        // Origin is outside of the BD plane
        // C is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointC);
        direction = -BDnormal;
        originInsideSimplex = false;
    }
    else if(DCdistance < 0.0f)
    {
        // Origin is outside of the DC plane
        // C is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointB);
        direction = -DCnormal;
        originInsideSimplex = false;
    }
    return originInsideSimplex;
}

bool CollisionSolver::AreConvexHullsColliding(const CollisionMesh& particle, 
                                              const CollisionMesh& hull, 
                                              Simplex& simplex)
{
    // If two convex hulls have collided, the Minkowski Sum A + (-B) of both 
    // hulls will contain the origin. Reference from 'Proximity Queries and 
    // Penetration Depth Computation on 3D Game Objects' by Gino van den Bergen
    // http://graphics.stanford.edu/courses/cs468-01-fall/Papers/van-den-bergen.pdf

    const std::vector<D3DXVECTOR3>& particleVertices = particle.GetVertices();
    const std::vector<D3DXVECTOR3>& hullVertices = hull.GetVertices();

    // Determine an initial point for the simplex
    const int initialIndex = 0;
    D3DXVECTOR3 direction = particleVertices[initialIndex] - hullVertices[initialIndex];
    D3DXVECTOR3 lastEdgePoint = GetMinkowskiSumEdgePoint(direction, particle, hull);
    simplex.AddPoint(lastEdgePoint);
        
    direction = -direction;
    int iteration = 0;
    bool collisionFound = false;
    bool collisionPossible = true;
    const int maxIterations = 20;

    // Iteratively create a simplex within the Minkowski Sum Hull
    while(iteration < maxIterations && !collisionFound && collisionPossible)
    {
        ++iteration;
        lastEdgePoint = GetMinkowskiSumEdgePoint(direction, particle, hull);
        simplex.AddPoint(lastEdgePoint);

        if(D3DXVec3Dot(&lastEdgePoint, &direction) <= 0)
        {
            // New edge point of simplex is not past the origin.
            collisionPossible = false;
        }
        else if(simplex.IsLine())
        {
            SolveLineSimplex(simplex, direction);
        }
        else if(simplex.IsTriPlane())
        {
            SolvePlaneSimplex(simplex, direction);
        }
        else if(simplex.IsTetrahedron())
        {
            collisionFound = SolveTetrahedronSimplex(simplex, direction);
        }
    }
    return collisionFound;
}