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;
}
}
}
}
void OjaData::get_oja_and_gradient(const Point& x,double& oja
,Point& grad) const
{
errif(x.dim() != dim(),"OjaData::get_oja_and_gradient: point " << x
<< " has illegal dimension");
if(plane)
{
plane->oja_and_gradient(x,oja,grad);
return;
}
double sum = 0.0;
double sgn;
double k = 1.0 / double(fact(dim()));
Simplex S;
Point Gr(dim());
for(Index I(dim(),size()); I; I++)
{
S.get(*this,I,x);
sum += S.size();
sgn = S.sign();
for(int j=0; j<dim(); j++)
Gr[j] += S.row_cof(j+1) * sgn * k;
}
oja = sum;
grad = Gr;
}
double Handler::optimise_single(vector<double> ¶meters, 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> ¶meters, 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;
}
//! Initialize the Paving with all points and multiplex
void init() {
generate_point();
Simplex<N> container;
for (size_t i = 0; i < N + 1; i++) {
container.pushPoint(multiPoints[i]);
}
multiplex.push_back(container);
for (size_t i = N + 1; i < multiPoints.size(); i++) {
for (size_t j = 0; j < multiplex.size(); j++) {
if (isInside(multiPoints[i], multiplex[j])) {
replaceSimplex(multiPoints[i], multiplex[j]);
break;
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
////
//// SUPPORT FUNCTION
////
/////////////////////////////////////////////////////////////////////////////////////////////
//
//
Vec3f Support(vector<Vec3f>& A, vector<Vec3f>& B, Vec3f v, Simplex& P){
Vector3f d(v[0], v[1], v[2]);
double farthestADir = Vector3f(A[0][0], A[0][1], A[0][2]).dot(d);
unsigned int farthestAIndex = 0;
for(unsigned int i = 1; i<A.size(); i++){
float newDot = Vector3f(A[i][0], A[i][1], A[i][2]).dot(d);
if(newDot > farthestADir){
farthestADir = newDot;
farthestAIndex = i;
}
}
d = -d;
double farthestBDir = ConvertToEigen3Vector(B[0]).dot(d);
unsigned int farthestBIndex = 0;
for(unsigned int i = 1; i<B.size(); i++){
float newDot = ConvertToEigen3Vector(B[i]).dot(d);
if(newDot > farthestBDir){
farthestBDir = newDot;
farthestBIndex = i;
}
}
float D = (ConvertToEigen3Vector(-A[farthestAIndex])).dot(ConvertToEigen3Vector(B[farthestBIndex]) - ConvertToEigen3Vector(A[farthestAIndex]));
P.AddAandB(A[farthestAIndex], B[farthestBIndex]);
return (A[farthestAIndex]-B[farthestBIndex]);
}
Image* GenerateWoodSide(int width, int height){
// Initialize Variables:
Image *terrain = new Image(width, height); // Output Map
double xValue, yValue, turb, distValue, sineValue;
int value;
const double PI = 3.141592653589793238463;
double xyFactor = 2.0; //Rings
double turbFactor = 1.0 / 800.0;
double turbPower = 0.05;
int octaves = 10;
// Initialize Simplex Generator:
srand(time(0) ^ rand()); // Initialize Random Seed
Simplex generator = Simplex();
// Loops through all the pixels
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
xValue = (x + width) / ((double)width);
yValue = (y - height / 2) / ((double)height);
turb = turbPower*generator.Turbulence(x*turbFactor, y*turbFactor, octaves);
distValue = (sqrt(xValue * xValue + yValue * yValue) + turb);
sineValue = 128 * fabs(sin(2 * distValue*PI*xyFactor));
// Convert the 0.0 to 1.0 noise value to a 0 to 255 RGB value:
value = int(sineValue);
if (value > 128)
value = 128;
else if (value < 0)
value = 0;
// Output heightmap to image:
(*terrain)[y][x].red = 80 + value;
(*terrain)[y][x].green = 30 + value;
(*terrain)[y][x].blue = 30;
}
}
return terrain;
}
void CollisionSolver::SolvePlaneSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
const D3DXVECTOR3& pointA = simplex.GetPoint(2);
const D3DXVECTOR3& pointB = simplex.GetPoint(0);
const D3DXVECTOR3& pointC = simplex.GetPoint(1);
const D3DXVECTOR3 AB = pointB - pointA;
const D3DXVECTOR3 AC = pointC - pointA;
const D3DXVECTOR3 AO = -pointA;
// Determine which side of the plane the origin is on
D3DXVECTOR3 planeNormal;
D3DXVec3Cross(&planeNormal, &AB, &AC);
// Determine the new search direction towards the origin
const float distanceToPlane = D3DXVec3Dot(&planeNormal, &AO);
direction = (distanceToPlane < 0.0f) ? -planeNormal : planeNormal;
}
double OjaData::oja(const Point& x) const
{
if (size() == 0) return -1;
errif(x.dim() != dim(),"OjaData::oja: Illegal dimension on point " << x);
if(plane)
return plane->oja(x);
double sum = 0.0;
Simplex S;
for(Index I(dim(),size()); I; I++)
{
S.get(*this,I,x);
sum += S.size();
}
return sum;
}
void CollisionSolver::SolveParticleHullCollision(CollisionMesh& particle,
const CollisionMesh& hull)
{
// Determine if within a rough radius of the convex hull
const D3DXVECTOR3 sphereToParticle = particle.GetPosition() - hull.GetPosition();
const float lengthSqr = D3DXVec3LengthSq(&sphereToParticle);
const float extendedParticleRadius = particle.GetRadius() * 2.0f;
const float combinedRadius = hull.GetRadius() + extendedParticleRadius;
if (lengthSqr < (combinedRadius*combinedRadius))
{
Simplex simplex;
if(AreConvexHullsColliding(particle, hull, simplex))
{
simplex.GenerateFaces();
const D3DXVECTOR3 penetration = GetConvexHullPenetration(particle, hull, simplex);
particle.ResolveCollision(penetration, hull.GetVelocity(), hull.GetShape());
}
}
}
/**
* @param separatingDistance Separating distance of two objects. In case of collision, the distance is zero.
*/
template<class T> GJKResultNoCollide<T>::GJKResultNoCollide(T separatingDistance, const Simplex<T> &simplex) :
GJKResult<T>(),
separatingDistance(separatingDistance),
simplex(simplex)
{
simplex.computeClosestPoints(closestPointA, closestPointB);
#ifdef _DEBUG
const float subtractDistance = closestPointA.vector(closestPointB).squareLength() - separatingDistance*separatingDistance;
assert((subtractDistance-0.01) <= 0.0 && (subtractDistance+0.01) >= 0.0);
#endif
}
bool operator()(const Simplex& s1, const Simplex& s2) const
{
if (s1.dimension() == s2.dimension())
return s1.get_value() < s2.get_value();
else
return s1.dimension() < s2.dimension();
}
Vec3f GJKDistance(vector<Vec3f>&A, vector<Vec3f>&B, Simplex& P){
P.clearSimplex();
Vec3f v= Support(A, B, A[0] - B[0],P);
P.Add(v);
v = ClosestIn(P);
float lastDist = FLT_MAX;
Simplex lastP;
lastP.SetToSimplex(P);
Vec3f lastV = v;
float epsilon = 0.1;
while(true){
float dist = v.norm();
Vec3f w = Support(A, B, -v, P);
Vector3f vE(v[0], v[1], v[2]);
Vector3f wE(w[0], w[1], w[2]);
float f = dist - (dist - w.norm());
if(f<= tolerance*dist || dist<tolerance){
return v;
}if(lastDist-dist<= epsilon*lastDist){
P.SetToSimplex(lastP);
return lastV;
}else{
lastP.SetToSimplex(P);
lastV = v;
}
if(P.alreadyIn(w))
return v;
if(vE.dot(wE) > 0)return v;
P.Add(w);
v = ClosestIn(P);
P.DeleteNonClosestIn();
if(P.GetSize() > 3) return v; //Should never reach here.
}
return v;
}
void CollisionSolver::SolveLineSimplex(const Simplex& simplex, D3DXVECTOR3& direction)
{
const D3DXVECTOR3& pointA = simplex.GetPoint(1);
const D3DXVECTOR3& pointB = simplex.GetPoint(0);
const D3DXVECTOR3 AB = pointB - pointA;
const D3DXVECTOR3 AO = -pointA;
// Generate a new direction for the next point
// perpendicular to the line using triple product
D3DXVECTOR3 ABcrossAO;
D3DXVec3Cross(&ABcrossAO, &AB, &AO);
D3DXVec3Cross(&direction, &ABcrossAO, &AB);
}
void MainWindow::sort_simplex(Simplex &smplx)
{
unsigned ar_size = smplx.get_number_of_vertexes(); //sort
for (unsigned j=0; j<ar_size-1; j++)
for (unsigned i=0; i<ar_size-1; i++)
if (func(smplx.get_vertex(i)) > func(smplx.get_vertex(i+1)))
std::swap(smplx.m_array[i], smplx.m_array[i+1]);
smplx.set_h(smplx.get_vertex(ar_size-1)); //set h,g,l
smplx.set_g(smplx.get_vertex(ar_size-2));
smplx.set_l(smplx.get_vertex(0));
Point c; //calculate center of mass
for (unsigned i=0; i<ar_size-1; i++)
c += smplx.m_array[i];
// c = std::accumulate(m_array.begin(), m_array.end() - 1, 0) /(ar_size - 1);
c /= ar_size-1;
smplx.set_c(c);
// std::sort(m_array.begin(), m_array.end(), comparator_by_f);
}
Point OjaData::gradient(const Point& x) const
{
errif(x.dim() != dim(),"OjaData::gradient: Illegal dimension on point " << x);
if(plane)
return plane->gradient(x);
Point Gr(dim());
Simplex S;
double sgn;
double k = 1.0 / double(fact(dim()));
for(Index I(dim(),size()); I; I++)
{
S.get(*this,I,x);
sgn = S.sign();
for(int j=0; j<dim(); j++)
Gr[j] += S.row_cof(j+1) * sgn * k;
}
return Gr;
}
void CollisionSolver::UpdateDiagnostics(const Simplex& simplex,
const D3DXVECTOR3& furthestPoint)
{
if(m_engine->diagnostic()->AllowDiagnostics(Diagnostic::COLLISION))
{
const float radius = 0.1f;
const float normalLength = 1.5f;
D3DXVECTOR3 origin(0.0, 0.0, 0.0);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"OriginPoint", Diagnostic::WHITE, origin, radius);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"FurthestPoint", Diagnostic::MAGENTA, furthestPoint, radius);
const auto& borders = simplex.GetBorderEdges();
for(unsigned int i = 0; i < borders.size(); ++i)
{
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"BorderEdge" + StringCast(i), Diagnostic::RED,
simplex.GetPoint(borders[i].indices[0]),
simplex.GetPoint(borders[i].indices[1]));
}
const auto& faces = simplex.GetFaces();
for(unsigned int i = 0; i < faces.size(); ++i)
{
if(faces[i].alive)
{
std::string id = StringCast(i);
const Face& face = faces[i];
const D3DXVECTOR3 center = simplex.GetFaceCenter(i);
const D3DXVECTOR3& normal = face.normal * normalLength;
const D3DXVECTOR3& pointA = simplex.GetPoint(face.indices[0]);
const D3DXVECTOR3& pointB = simplex.GetPoint(face.indices[1]);
const D3DXVECTOR3& pointC = simplex.GetPoint(face.indices[2]);
m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
"sCenter" + id, Diagnostic::BLUE, center, radius);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sNormal" + id, Diagnostic::BLUE, center, center + normal);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine1" + id, Diagnostic::YELLOW, pointA, pointB);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine2" + id, Diagnostic::YELLOW, pointA, pointC);
m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
"sLine3" + id, Diagnostic::YELLOW, pointC, pointB);
}
}
}
}
void add_cofaces (const Graph& graph, Simplex& tau,
const Neighbors& neighbors,
Complex& complex, const std::size_t dimension) {
typedef typename Neighbors::const_iterator Neighbor_iterator;
complex.insert_open_cell(tau);
if(tau.dimension() >= dimension) { return; }
Neighbors lower_neighbors;
Neighbors final_neighbors;
for(Neighbor_iterator i = neighbors.begin(); i != neighbors.end(); ++i){
lower_neighbors.clear();
Simplex sigma( tau);
sigma.insert( *i);
get_lower_neighbors(graph, *i, lower_neighbors);
final_neighbors.clear();
set_intersection(lower_neighbors.begin(),lower_neighbors.end(),
neighbors.begin(),neighbors.end(),
back_inserter(final_neighbors));
add_cofaces(graph, sigma, final_neighbors, complex, dimension);
}
}
