//---------------------------------------------------------
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 );
}
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];
}
*/
}
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";
}
}
}
} // }}}
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;
}
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;
}
