int main(){
//Body Init=================+
//Earth =========NAME==MASS===POS====VEL======+
Body<double> earth("Earth", 5.97e24, {10, 10, 0}, {0, 0, 0});
bodies.push_back(earth);
//Satellite =======NAME=MASS===POS======VEL=======+ 7545.687
Body<double> sat("Benley", 5000, {7e6, 0, 0}, {0, 7545.687, 0});
bodies.push_back(sat);
//End Body Init=============+
std::vector<std::vector<double>> forcesBro = netForce();
for(int i = 0; i < forcesBro.size(); ++i){
for(int j = 0; j < 3; ++j){
std::cout << forcesBro[i][j] << " ";
}
std::cout << '\n';
}
//Time init
double t = 0.0, dt = 0.2, ElapT=0.0;
std::cout << "bodies.size(): " << bodies.size() << std::endl;
//File output
std::ofstream fout("SystemStates.txt");
//Integration Loop
std::vector<double> posNewTemp = {0, 0, 0};
std::vector<double> velNewTemp = {0, 0, 0};
std::vector<double> posTemp = {0, 0, 0};
std::vector<double> velTemp = {0, 0, 0};
//double massTemp = 0;
for(double i=0; i<100000; ++i){ // Full propagated iteration
//Running only with sat
for(size_t j=0;j<bodies.size();++j){ // Per-body propagation
// Reassign Temps
posTemp = bodies[j].getPos();
velTemp = bodies[j].getVel();
//massTemp = bodies[j].getMass();
// Integrate
prop(posTemp, velTemp, dt, (j-1)%2);
// setPos and setVel Update
bodies[j].setPosNew(posTemp);
bodies[j].setVelNew(velTemp);
}
// Update time
ElapT += dt;
t += dt;
// Update positions
updateStates();
// File output
fout << bodies[1].getPos()[0] << '\t' << bodies[1].getPos()[1] << '\t' << bodies[1].getPos()[2] << '\t'
<< bodies[0].getPos()[0] << '\t' << bodies[0].getPos()[1] << '\t' << bodies[0].getPos()[2] << '\t'
<< bodies[1].getVel()[0] << '\t' << bodies[1].getVel()[1] << '\t' << bodies[1].getVel()[2] << '\t'
<< ElapT << '\n';
}
return 0;
}
void
verifyProcrustes (const std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T> >& from,
const std::vector<IMATH_INTERNAL_NAMESPACE::Vec3<T> >& to)
{
typedef IMATH_INTERNAL_NAMESPACE::Vec3<T> V3;
const T eps = std::sqrt(std::numeric_limits<T>::epsilon());
const size_t n = from.size();
// Validate that passing in uniform weights gives the same answer as
// passing in no weights:
std::vector<T> weights (from.size());
for (size_t i = 0; i < weights.size(); ++i)
weights[i] = 1;
IMATH_INTERNAL_NAMESPACE::M44d m1 = procrustesRotationAndTranslation (&from[0], &to[0], n);
IMATH_INTERNAL_NAMESPACE::M44d m2 = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
assert (std::abs(m1[i][j] - m2[i][j]) < eps);
// Now try the weighted version:
for (size_t i = 0; i < weights.size(); ++i)
weights[i] = i+1;
IMATH_INTERNAL_NAMESPACE::M44d m = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);
// with scale:
IMATH_INTERNAL_NAMESPACE::M44d ms = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n, true);
// Verify that it's orthonormal w/ positive determinant.
const T det = m.determinant();
assert (std::abs(det - T(1)) < eps);
// Verify orthonormal:
IMATH_INTERNAL_NAMESPACE::M33d upperLeft;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
upperLeft[i][j] = m[i][j];
IMATH_INTERNAL_NAMESPACE::M33d product = upperLeft * upperLeft.transposed();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
const double expected = (i == j ? 1.0 : 0.0);
assert (std::abs(product[i][j] - expected) < eps);
}
}
// Verify that nearby transforms are worse:
const size_t numTries = 10;
IMATH_INTERNAL_NAMESPACE::Rand48 rand (1056);
const double delta = 1e-3;
for (size_t i = 0; i < numTries; ++i)
{
// Construct an orthogonal rotation matrix using Euler angles:
IMATH_INTERNAL_NAMESPACE::Eulerd diffRot (delta * rand.nextf(), delta * rand.nextf(), delta * rand.nextf());
assert (procrustesError (&from[0], &to[0], &weights[0], n, m * diffRot.toMatrix44()) >
procrustesError (&from[0], &to[0], &weights[0], n, m));
// Try a small translation:
IMATH_INTERNAL_NAMESPACE::V3d diffTrans (delta * rand.nextf(), delta * rand.nextf(), delta * rand.nextf());
IMATH_INTERNAL_NAMESPACE::M44d translateMatrix;
translateMatrix.translate (diffTrans);
assert (procrustesError (&from[0], &to[0], &weights[0], n, m * translateMatrix) >
procrustesError (&from[0], &to[0], &weights[0], n, m));
}
// Try a small scale:
IMATH_INTERNAL_NAMESPACE::M44d newMat = ms;
const double scaleDiff = delta;
for (size_t i = 0; i < 3; ++i)
for (size_t j = 0; j < 3; ++j)
newMat[i][j] = ms[i][j] * (1.0 + scaleDiff);
assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
procrustesError (&from[0], &to[0], &weights[0], n, ms));
for (size_t i = 0; i < 3; ++i)
for (size_t j = 0; j < 3; ++j)
newMat[i][j] = ms[i][j] * (1.0 - scaleDiff);
assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
procrustesError (&from[0], &to[0], &weights[0], n, ms));
//
// Verify the magical property that makes shape springs work:
// when the displacements Q*A-B, times the weights,
// are applied as forces at B,
// there is zero net force and zero net torque.
//
{
IMATH_INTERNAL_NAMESPACE::V3d center (0, 0, 0);
IMATH_INTERNAL_NAMESPACE::V3d netForce(0);
IMATH_INTERNAL_NAMESPACE::V3d netTorque(0);
for (int iPoint = 0; iPoint < n; ++iPoint)
{
const IMATH_INTERNAL_NAMESPACE::V3d force = weights[iPoint] * (from[iPoint]*m - to[iPoint]);
netForce += force;
netTorque += to[iPoint].cross (force);
}
assert (netForce.length2() < eps);
assert (netTorque.length2() < eps);
}
}
