//---------------------------------------------------------
bool CGSPoints_Distances::On_Execute(void)
{
    //-----------------------------------------------------
    CSG_Shapes	*pPoints	= Parameters("POINTS")	->asShapes();
    CSG_Table	*pTable		= Parameters("TABLE")	->asTable();

    //-----------------------------------------------------
    CSG_PRQuadTree			QT(pPoints, 0);
    CSG_Simple_Statistics	s;

    double	x, y, z;

    for(int iPoint=0; iPoint<pPoints->Get_Count() && Set_Progress(iPoint, pPoints->Get_Count()); iPoint++)
    {
        TSG_Point	p	= pPoints->Get_Shape(iPoint)->Get_Point(0);

        if( QT.Select_Nearest_Points(p.x, p.y, 2) && QT.Get_Selected_Point(1, x, y, z) && (x != p.x || y != p.y) )
        {
            s.Add_Value(SG_Get_Distance(x, y, p.x, p.y));
        }
    }

    //-----------------------------------------------------
    if( s.Get_Count() > 0 )
    {
        CSG_Table_Record	*pRecord;

        pTable->Destroy();
        pTable->Set_Name(CSG_String::Format(SG_T("%s [%s]"), _TL("Minimum Distance Analysis"), pPoints->Get_Name()));

        pTable->Add_Field(SG_T("NAME")	, SG_DATATYPE_String);
        pTable->Add_Field(SG_T("VALUE")	, SG_DATATYPE_Double);

        SET_VALUE(_TL("Mean Average")		, s.Get_Mean());
        SET_VALUE(_TL("Minimum")			, s.Get_Minimum());
        SET_VALUE(_TL("Maximum")			, s.Get_Maximum());
        SET_VALUE(_TL("Standard Deviation")	, s.Get_StdDev());
        SET_VALUE(_TL("Duplicates")			, pPoints->Get_Count() - s.Get_Count());

        DataObject_Update(pTable, SG_UI_DATAOBJECT_SHOW);

        return( true );
    }

    Message_Dlg(_TL("not enough observations"));

    return( false );
}
示例#2
0
void QRDecomp(MAT &A,MAT &Q, MAT &R){
    int n=A.dim();
    MAT AT(A.tpose());
    MAT QT(Q.tpose());
    VEC S(n);                   //sum vector
    R[0][0]=sqrt(AT[0]*AT[0]);  //initial R
    QT[0]=AT[0]/R[0][0];                //initial Q

    for(int j=1;j<n;j++){
        for(int i=0;i<n;i++){
            S[i] = 0;
        }       //initialization of sum vector
        for(int i=0;i<=j;i++){
                R[i][j] = QT[i]*AT[j];
        }
        for(int i=0;i<=j-1;i++){
                S = S + R[i][j]*QT[i];          //do the summation
        }
        S = AT[j] - S;
        R[j][j]=sqrt(S*S);
        QT[j] = S/R[j][j];
    }
    Q=QT.tpose();

	
	
	/*
    for(int j=1;j<n;j++){
        
        for(int i=0;i<=j;i++){
                R[i][j] = QT[i]*AT[j];
        }
        for(int i=0;i<=j-1;i++){
                AT[j] = AT[j] - (R[i][j]*QT[i]);          //do the summation
        }
        
        R[j][j]=std::sqrt(AT[j]*AT[j]);
        QT[j] = AT[j]/R[j][j];
    }
    */

}	
示例#3
0
int main(int argc, char* argv[]) {

	sf::RenderWindow window(sf::VideoMode(WIDTH, HEIGHT), "SpaceGameThing");
	sf::Clock clock;
	sf::Clock physClock;

	float dt;

	HeavenlyBody* planet = new HeavenlyBody(0.25, 25000, QUIET);
	planet->Load("../bin/planet.png");
	planet->SetPosition(WIDTH/2, 95);
	planet->SetCollidable(true);
	//planet->SetOrigin(95,95);
	planet->SetColor(sf::Color(255, 0, 0, 255));
	HitBoxBase<std::pair<sf::Vector2f, float> > hbox(std::pair<sf::Vector2f, float>(sf::Vector2f(95, 95), planet->GetRadius()));
	planet->SetHitBox((void*)(&hbox), collision::RADIAL);

	HeavenlyBody* planet2 = new HeavenlyBody(2.0, 200000, QUIET);
	planet2->Load("../bin/planet.png");
	planet2->SetPosition(WIDTH/2, HEIGHT+90);
	planet2->SetCollidable(true);
	//planet2->SetOrigin(95,95);
	planet2->SetColor(sf::Color(255, 0, 0, 255));
	HitBoxBase<std::pair<sf::Vector2f, float> > hbox2(std::pair<sf::Vector2f, float>(sf::Vector2f(95, 95), planet2->GetRadius()));
	planet2->SetHitBox((void*)(&hbox2), collision::RADIAL);


	HeavenlyBody* planet3 = new HeavenlyBody(0.5, 20000, QUIET);
	planet3->Load("../bin/planet.png");
	planet3->SetPosition(WIDTH/4, 90);
	planet3->SetCollidable(true);
	//planet3->SetOrigin(95,95);
	planet3->SetColor(sf::Color(255, 0, 0, 255));
	HitBoxBase<std::pair<sf::Vector2f, float> > hbox3(std::pair<sf::Vector2f, float>(sf::Vector2f(95, 95), planet3->GetRadius()));
	planet3->SetHitBox((void*)(&hbox3), collision::RADIAL);



	//test a compound asset
	CompoundAsset* cst = new CompoundAsset();
	cst->Load("../bin/scripts/assets/test.ass");
	cst->SetOrigin(256, 256);
	cst->SetPosition(WIDTH/2, HEIGHT/2);
	cst->setMass(100000.0);
	cst->SetScale(0.25, 0.25);

	AssetManager manager;
	PhysicsManager physManager;
	
	manager.Add("planet", planet);
	manager.Add("planet2", planet2);
	manager.Add("planet3", planet3);
	manager.Add("cst", cst);
	//manager.Add("redPlanet", surface);
	physManager.Add(cst);
	physManager.Add(planet);
	physManager.Add(planet2);
	physManager.Add(planet3);
	physManager.InitPhysVec();

	QuadTree QT(-3/2*WIDTH, 0, 3*WIDTH, HEIGHT); 
	//QT.AddGeometry(*surface);

	WorldGeometry* Geoms[30];
	std::string name = "surface";
	std::string index;
	std::string fin;
	std::stringstream num (std::stringstream::in | std::stringstream::out);
	for (int i = 0; i < 30; ++i) {
		Geoms[i] = new WorldGeometry();
		Geoms[i]->Load("../bin/planet.png");
		Geoms[i]->SetPosition(192*(i-15), HEIGHT - 190);
		Geoms[i]->SetCollidable(true);
		Geoms[i]->SetOrigin(95,95);
		QT.AddGeometry(*Geoms[i]);
		num << i;
		index = num.str();
		num.str("");
		fin = name + index;
		manager.Add(fin, Geoms[i]);
	}
	/*
	WorldGeometry* surface = new WorldGeometry();
	surface->Load("../bin/planet.png");
	surface->SetPosition(0, HEIGHT-190);
	surface->SetCollidable(true);
	surface->SetOrigin(95,95);
	*/



	planet->setVx(-180);
	planet->setVy(-40.0);
	planet2->setVx(50);
	int nFrames = 0;
	sf::Clock fClock;
	sf::Font font; //make a frame counter in a class later
	font.loadFromFile("../bin/DroidSans.tff");
	sf::Text Fps("0", font, 14);
	std::stringstream ss (std::stringstream::in | std::stringstream::out);

	std::set<WorldGeometry*> geoms;
	std::set<WorldGeometry*>::iterator geom;
	std::vector<HeavenlyBody*> bodies;
	bodies.push_back(planet);
	bodies.push_back(planet2);
	bodies.push_back(planet3);
	while (window.isOpen()) {
	
		sf::Event event;
		while (window.pollEvent(event)) {
			//player1->Interact(event); //shouldn't do this here
			if (event.type == sf::Event::Closed)
				window.close();
		}
		physManager.UpdatePhysics(physClock.restart().asSeconds());
	
		//quadTree experimentation
	
		for (int b = 0; b < 3; ++b) {	
			HeavenlyBody* plnt = bodies[b];
			for (int d = 0; d < 3; ++d) {
				if (d != b) {
					HeavenlyBody* other = bodies[d];
					//if (HaveCollided(plnt, other)) {
					if (plnt->HasCollided(other)) {

						printf("Collision Detected\n");
						//first, set up the change in radial velocity
						sf::Vector2f r = other->GetPosition() - plnt->GetPosition();
						float R = hypotf(r.x,r.y);
						sf::Vector2f sxy = other->GetPosition();
						sf::Vector2f pxy = plnt->GetPosition();
						sf::Vector2f v = (plnt->GetVelocity()*plnt->getMass() - other->GetVelocity()*other->getMass()); // dealta v (this is a diffrence of v)
						sf::Vector2f v2 = other->GetVelocity()*other->getMass(); // dealta v (this is a diffrence of v)
						sf::Vector2f v1 = plnt->GetVelocity()*plnt->getMass(); // dealta v (this is a diffrence of v)
						float damping = 0.85; //damping factor	
						sf::Vector2f frict = v;
						v1 -= r*(2.0f*(v.x*r.x+v.y*r.y)/(R*R)*damping);
						v2 += r*(2.0f*(v.x*r.x+v.y*r.y)/(R*R)*damping);
						//next, set up the change in tangential velocity

						v1 -= r*((frict.x*r.y-frict.y*r.x)/(R*R)*(1-damping)); //not real friction, 
						v2 += r*((frict.x*r.y-frict.y*r.x)/(R*R)*(1-damping)); //not real friction, 
						//but something at least, this can be fixed later

						//lastly, set up the change in angular momentum

						float omega, omega2;
						//sf::Vector2f delV = r*((frict.x*r.y-frict.y*r.x)/(R*R)*(1-damping)); //not real friction, 
						sf::Vector2f delV = frict*(1-damping); //not real friction, 
						omega = (1-damping)*(delV.x*r.x+delV.y*r.y/(R*R));//*plnt->getMass()/plnt->getI();
						omega2 = -(1-damping)*(delV.x*r.x+delV.y*r.y/(R*R));//*plnt->getMass()/plnt->getI();

						//then set the physics
						//plnt->setVx(v1.x/plnt->getMass());
						//plnt->setVy(v1.y/plnt->getMass());
						//plnt->setOmega(omega);
						plnt->setX(sxy.x-(95*other->GetRadius()+95*plnt->GetRadius())*(r.x)/R);
						plnt->setY(sxy.y-(95*other->GetRadius()+95*plnt->GetRadius())*(r.y)/R);
		
						//other->setVx(v2.x/other->getMass());
						//other->setVy(v2.y/other->getMass());
						//other->setOmega(omega2);
						//other->setX(pxy.x+(95*plnt->GetRadius()+95*other->GetRadius())*(r.x)/R);
						//other->setY(pxy.y+(95*plnt->GetRadius()+95*other->GetRadius())*(r.y)/R);

					}
				}
			}
			geoms = QT.GetContents(plnt->GetGlobalBounds());
			if (geoms.size() != 0) {
				for (geom = geoms.begin(); geom != geoms.end(); ++geom) {
					WorldGeometry* surface = *geom; //get out collider
					sf::Vector2f r = surface->GetPosition() - plnt->GetPosition(); //dealta r
					float R = hypotf(r.x,r.y);
					if (R < 95+plnt->GetRadius()*95.0) {

						//first, set up the change in radial velocity
						sf::Vector2f sxy = surface->GetPosition();
						sf::Vector2f v = plnt->GetVelocity(); // dealta v (this is a diffrence of v)
						float damping = 0.85; //damping factor	
						sf::Vector2f frict = v;
						v -= r*(2.0f*(v.x*r.x+v.y*r.y)/(R*R)*damping);
						//next, set up the change in tangential velocity

						v -= r*((frict.x*r.y-frict.y*r.x)/(R*R)*(1-damping)); //not real friction, 
						//but something at least, this can be fixed later

						//lastly, set up the change in angular momentum

						float omega;
						//sf::Vector2f delV = r*((frict.x*r.y-frict.y*r.x)/(R*R)*(1-damping)); //not real friction, 
						sf::Vector2f delV = frict*(1-damping); //not real friction, 
						omega = (1-damping)*(delV.x*r.x+delV.y*r.y/(R*R));//*plnt->getMass()/plnt->getI();

						//then set the physics
						plnt->setVx(v.x);
						plnt->setVy(v.y);
						plnt->setOmega(omega);
						plnt->setX(sxy.x-(95+95*plnt->GetRadius())*(r.x)/R);
						plnt->setY(sxy.y-(95+95*plnt->GetRadius())*(r.y)/R);
					}
				}
			}
		}
	
		//

		if (clock.getElapsedTime().asSeconds() > 1.0/FPS) {
			clock.restart();
			window.clear();
			nFrames += 1;
			manager.DrawAll(window);
			window.draw(Fps);
			window.display();
		}
		++nFrames;
		if (fClock.getElapsedTime().asSeconds() > 2.0){
			double fps = nFrames/fClock.restart().asSeconds();
			nFrames = 0;
			ss << fps;
			Fps.setString(ss.str());
			ss.str("");	
		}
	}
	return 0;
}
void check_QR(
    orthotope<T> const& A
  , orthotope<T> const& Q
  , orthotope<T> const& R
    )
{ // {{{
    BOOST_ASSERT(2 == A.order());
    BOOST_ASSERT(2 == Q.order());
    BOOST_ASSERT(2 == R.order());
    BOOST_ASSERT(A.hypercube());
    BOOST_ASSERT(Q.hypercube());
    BOOST_ASSERT(R.hypercube());

    std::size_t const n = A.extent(0);

    BOOST_ASSERT(n == Q.extent(0));
    BOOST_ASSERT(n == R.extent(0));

    ///////////////////////////////////////////////////////////////////////////
    /// Make sure Q * R equals A.
    orthotope<T> QR = matrix_multiply(Q, R);

    for (std::size_t l = 0; l < n; ++l)
    {
        for (std::size_t i = 0; i < n; ++i)
        {
            if (!compare_floating(A(l, i), QR(l, i), 1e-6)) 
                std::cout << "WARNING: QR[" << l << "][" << i << "] (value "
                          << QR(l, i) << ") is not equal to A[" << l << "]["
                          << i << "] (value " << A(l, i) << ")\n";
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    /// Make sure R is an upper triangular matrix. 
    for (std::size_t l = 0; l < (n - 1); ++l)
    {
        for (std::size_t i = l + 1; i < n; ++i)
        {
            if (!compare_floating(0.0, R(i, l), 1e-6))
                std::cout << "WARNING: R[" << i << "][" << l << "] is not 0 "
                             "(value is " << R(i, l) << "), R is not an upper "
                             "triangular matrix\n";
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    /// Make sure Q is orthogonal. A matrix is orthogonal if its transpose is
    /// equal to its inverse:
    ///
    ///     Q^T = Q^-1
    ///
    /// This implies that:
    ///
    ///     Q^T * Q = Q * Q^T = I
    /// 
    /// We use the above formula to verify Q's orthogonality. 
    orthotope<T> QT = Q.copy();

    // Transpose QT.
    for (std::size_t l = 0; l < (n - 1); ++l)
        for (std::size_t i = l + 1; i < n; ++i)
            std::swap(QT(l, i), QT(i, l));

    // Compute Q^T * Q and store the result in QT.
    QT = matrix_multiply(Q, QT);

    for (std::size_t l = 0; l < n; ++l)
    {
        for (std::size_t i = 0; i < n; ++i)
        {
            // Diagonals should be 1. 
            if (l == i)
            {
                if (!compare_floating(1.0, QT(l, i), 1e-6)) 
                    std::cout << "WARNING: (Q^T * Q)[" << l << "][" << i << "] "
                                 "is not 1 (value is " << QT(l, i) << "), Q is "
                                 "not an orthogonal matrix\n";
            }

            // All other entries should be 0.
            else
            {
                if (!compare_floating(0.0, QT(l, i), 1e-6)) 
                    std::cout << "WARNING: (Q^T * Q)[" << l << "][" << i << "] "
                                 "is not 0 (value is " << QT(l, i) << "), Q is "
                                 "not an orthogonal matrix\n";
            }
        }
    }
} // }}}
示例#5
0
文件: QDWH.hpp 项目: certik/Elemental
int QDWH
( DistMatrix<F>& A, 
  typename Base<F>::type lowerBound,
  typename Base<F>::type upperBound )
{
#ifndef RELEASE
    PushCallStack("QDWH");
#endif
    typedef typename Base<F>::type R;
    const Grid& g = A.Grid();
    const int height = A.Height();
    const int width = A.Width();
    const R oneHalf = R(1)/R(2);
    const R oneThird = R(1)/R(3);

    if( height < width )
        throw std::logic_error("Height cannot be less than width");

    const R epsilon = lapack::MachineEpsilon<R>();
    const R tol = 5*epsilon;
    const R cubeRootTol = Pow(tol,oneThird);

    // Form the first iterate
    Scale( 1/upperBound, A );

    int numIts=0;
    R frobNormADiff;
    DistMatrix<F> ALast( g );
    DistMatrix<F> Q( height+width, width, g );
    DistMatrix<F> QT(g), QB(g);
    PartitionDown( Q, QT,
                      QB, height );
    DistMatrix<F> C( g );
    DistMatrix<F> ATemp( g );
    do
    {
        ++numIts;
        ALast = A;

        R L2;
        Complex<R> dd, sqd;
        if( Abs(1-lowerBound) < tol )
        {
            L2 = 1;
            dd = 0;
            sqd = 1;
        }
        else
        {
            L2 = lowerBound*lowerBound;
            dd = Pow( 4*(1-L2)/(L2*L2), oneThird );
            sqd = Sqrt( 1+dd );
        }
        const Complex<R> arg = 8 - 4*dd + 8*(2-L2)/(L2*sqd);
        const R a = (sqd + Sqrt( arg )/2).real;
        const R b = (a-1)*(a-1)/4;
        const R c = a+b-1;
        const Complex<R> alpha = a-b/c;
        const Complex<R> beta = b/c;

        lowerBound = lowerBound*(a+b*L2)/(1+c*L2);

        if( c > 100 )
        {
            //
            // The standard QR-based algorithm
            //
            QT = A;
            Scale( Sqrt(c), QT );
            MakeIdentity( QB );
            ExplicitQR( Q );
            Gemm( NORMAL, ADJOINT, alpha/Sqrt(c), QT, QB, beta, A );
        }
        else
        {
            //
            // Use faster Cholesky-based algorithm since A is well-conditioned
            //
            Identity( width, width, C );
            Herk( LOWER, ADJOINT, F(c), A, F(1), C );
            Cholesky( LOWER, C );
            ATemp = A;
            Trsm( RIGHT, LOWER, ADJOINT, NON_UNIT, F(1), C, ATemp );
            Trsm( RIGHT, LOWER, NORMAL, NON_UNIT, F(1), C, ATemp );
            Scale( beta, A );
            Axpy( alpha, ATemp, A );
        }

        Axpy( F(-1), A, ALast );
        frobNormADiff = Norm( ALast, FROBENIUS_NORM );
    }
    while( frobNormADiff > cubeRootTol || Abs(1-lowerBound) > tol );
#ifndef RELEASE
    PopCallStack();
#endif
    return numIts;
}
示例#6
0
int Halley
( DistMatrix<F>& A, typename Base<F>::type upperBound )
{
#ifndef RELEASE
    PushCallStack("Halley");
#endif
    typedef typename Base<F>::type R;
    const Grid& g = A.Grid();
    const int height = A.Height();
    const int width = A.Width();
    const R oneHalf = R(1)/R(2);
    const R oneThird = R(1)/R(3);

    if( height < width )
        throw std::logic_error("Height cannot be less than width");

    const R epsilon = lapack::MachineEpsilon<R>();
    const R tol = 5*epsilon;
    const R cubeRootTol = Pow(tol,oneThird);
    const R a = 3;
    const R b = 1;
    const R c = 3;

    // Form the first iterate
    Scale( 1/upperBound, A );

    int numIts=0;
    R frobNormADiff;
    DistMatrix<F> ALast( g );
    DistMatrix<F> Q( height+width, width, g );
    DistMatrix<F> QT(g), QB(g);
    PartitionDown( Q, QT,
                      QB, height );
    DistMatrix<F> C( g );
    DistMatrix<F> ATemp( g );
    do
    {
        if( numIts > 100 )
            throw std::runtime_error("Halley iteration did not converge");
        ++numIts;
        ALast = A;

        // TODO: Come up with a test for when we can use the Cholesky approach
        if( true )
        {
            //
            // The standard QR-based algorithm
            //
            QT = A;
            Scale( Sqrt(c), QT );
            MakeIdentity( QB );
            ExplicitQR( Q );
            Gemm( NORMAL, ADJOINT, F(a-b/c)/Sqrt(c), QT, QB, F(b/c), A );
        }
        else
        {
            //
            // Use faster Cholesky-based algorithm since A is well-conditioned
            //
            Identity( width, width, C );
            Herk( LOWER, ADJOINT, F(c), A, F(1), C );
            Cholesky( LOWER, C );
            ATemp = A;
            Trsm( RIGHT, LOWER, ADJOINT, NON_UNIT, F(1), C, ATemp );
            Trsm( RIGHT, LOWER, NORMAL, NON_UNIT, F(1), C, ATemp );
            Scale( b/c, A );
            Axpy( a-b/c, ATemp, A );
        }

        Axpy( F(-1), A, ALast );
        frobNormADiff = Norm( ALast, FROBENIUS_NORM );
    }
    while( frobNormADiff > cubeRootTol );
#ifndef RELEASE
    PopCallStack();
#endif
    return numIts;
}