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); } }