Molecule H2()
{
int nAtoms = 2;
Eigen::Vector3d H1( 0.735000, 0.000000, 0.000000);
Eigen::Vector3d H2(-0.735000, 0.000000, 0.000000);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = H1.transpose();
geom.col(1) = H2.transpose();
Eigen::Vector2d charges, masses;
charges << 1.0, 1.0;
masses << 1.0078250, 1.0078250;
std::vector<Atom> atoms;
double radiusH = 1.20;
atoms.push_back( Atom("Hydrogen", "H", charges(0), masses(0), radiusH, H1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(1), masses(1), radiusH, H2, 1.0) );
std::vector<Sphere> spheres;
Sphere sph2(H1, radiusH);
Sphere sph3(H2, radiusH);
spheres.push_back(sph2);
spheres.push_back(sph3);
Symmetry pGroup = buildGroup(0, 0, 0, 0);
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
Molecule H3()
{
int nAtoms = 3;
Eigen::Vector3d H1( 0.735000, 0.000000, -1.333333);
Eigen::Vector3d H2(-0.735000, 0.000000, -1.333333);
Eigen::Vector3d H3( 0.000000, 0.000000, 2.666667);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = H1.transpose();
geom.col(1) = H2.transpose();
geom.col(2) = H3.transpose();
Eigen::Vector3d charges, masses;
charges << 1.0, 1.0, 1.0;
masses << 1.0078250, 1.0078250, 1.0078250;
std::vector<Atom> atoms;
double radiusH = (1.20 * 1.20) / convertBohrToAngstrom;
atoms.push_back( Atom("Hydrogen", "H", charges(0), masses(0), radiusH, H1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(1), masses(1), radiusH, H2, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(2), masses(2), radiusH, H3, 1.0) );
std::vector<Sphere> spheres;
Sphere sph2(H1, radiusH);
Sphere sph3(H2, radiusH);
Sphere sph4(H3, radiusH);
spheres.push_back(sph2);
spheres.push_back(sph3);
spheres.push_back(sph4);
enum pointGroup { pgC1, pgC2, pgCs, pgCi, pgD2, pgC2v, pgC2h, pgD2h };
Symmetry pGroup;
switch(group) {
case(pgC1):
pGroup = buildGroup(0, 0, 0, 0);
break;
case(pgC2v):
// C2v as generated by Oyz and Oxz
pGroup = buildGroup(2, 1, 2, 0);
break;
default:
pGroup = buildGroup(0, 0, 0, 0);
break;
}
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
Molecule NH3()
{
int nAtoms = 4;
Eigen::Vector3d N( -0.000000000, -0.104038047, 0.000000000);
Eigen::Vector3d H1(-0.901584415, 0.481847022, -1.561590016);
Eigen::Vector3d H2(-0.901584415, 0.481847022, 1.561590016);
Eigen::Vector3d H3( 1.803168833, 0.481847022, 0.000000000);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = N.transpose();
geom.col(1) = H1.transpose();
geom.col(2) = H2.transpose();
geom.col(3) = H3.transpose();
Eigen::Vector4d charges, masses;
charges << 7.0, 1.0, 1.0, 1.0;
masses << 14.0030740, 1.0078250, 1.0078250, 1.0078250;
std::vector<Atom> atoms;
atoms.push_back( Atom("Nitrogen", "N", charges(0), masses(0), 2.929075493, N,
1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(1), masses(1), 2.267671349, H1,
1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(2), masses(2), 2.267671349, H2,
1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(3), masses(3), 2.267671349, H3,
1.0) );
std::vector<Sphere> spheres;
Sphere sph1(N, 2.929075493);
Sphere sph2(H1, 2.267671349);
Sphere sph3(H2, 2.267671349);
Sphere sph4(H3, 2.267671349);
spheres.push_back(sph1);
spheres.push_back(sph2);
spheres.push_back(sph3);
spheres.push_back(sph4);
// C1
Symmetry pGroup = buildGroup(0, 0, 0, 0);
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
Molecule CH3()
{
int nAtoms = 4;
Eigen::Vector3d C1( 0.0006122714, 0.0000000000, 0.0000000000);
Eigen::Vector3d H1( 1.5162556382, -1.3708721537, 0.0000000000);
Eigen::Vector3d H2(-0.7584339548, 0.6854360769, 1.7695110698);
Eigen::Vector3d H3(-0.7584339548, 0.6854360769, -1.7695110698);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = C1.transpose();
geom.col(1) = H1.transpose();
geom.col(2) = H2.transpose();
geom.col(3) = H3.transpose();
Eigen::Vector4d charges, masses;
charges << 6.0, 1.0, 1.0, 1.0;
masses << 12.00, 1.0078250, 1.0078250, 1.0078250;
double radiusC = (1.70 * 1.20) / convertBohrToAngstrom;
double radiusH = (1.20 * 1.20) / convertBohrToAngstrom;
std::vector<Atom> atoms;
atoms.push_back( Atom("Carbon", "C", charges(0), masses(0), radiusC, C1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(1), masses(1), radiusH, H1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(2), masses(2), radiusH, H2, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(3), masses(3), radiusH, H3, 1.0) );
std::vector<Sphere> spheres;
Sphere sph1(C1, radiusC);
Sphere sph2(H1, radiusH);
Sphere sph3(H2, radiusH);
Sphere sph4(H3, radiusH);
spheres.push_back(sph1);
spheres.push_back(sph2);
spheres.push_back(sph3);
spheres.push_back(sph4);
// Cs as generated by Oxy
Symmetry pGroup = buildGroup(1, 4, 0, 0);
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
Molecule C2H4()
{
int nAtoms = 6;
Eigen::Vector3d C1(0.0000000000, 0.0000000000, 1.2578920000);
Eigen::Vector3d H1(0.0000000000, 1.7454620000, 2.3427160000);
Eigen::Vector3d H2(0.0000000000, -1.7454620000, 2.3427160000);
Eigen::Vector3d C2(0.0000000000, 0.0000000000, -1.2578920000);
Eigen::Vector3d H3(0.0000000000, 1.7454620000, -2.3427160000);
Eigen::Vector3d H4(0.0000000000, -1.7454620000, -2.3427160000);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = C1.transpose();
geom.col(1) = H1.transpose();
geom.col(2) = H2.transpose();
geom.col(3) = C2.transpose();
geom.col(4) = H3.transpose();
geom.col(5) = H4.transpose();
Eigen::VectorXd charges(6), masses(6);
charges << 6.0, 1.0, 1.0, 6.0, 1.0, 1.0;
masses << 12.00, 1.0078250, 1.0078250, 12.0, 1.0078250, 1.0078250;
double radiusC = (1.70 * 1.20) / convertBohrToAngstrom;
double radiusH = (1.20 * 1.20) / convertBohrToAngstrom;
std::vector<Atom> atoms;
atoms.push_back( Atom("Carbon", "C", charges(0), masses(0), radiusC, C1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(1), masses(1), radiusH, H1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(2), masses(2), radiusH, H2, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(3), masses(3), radiusC, C2, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(4), masses(4), radiusH, H3, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(5), masses(5), radiusH, H4, 1.0) );
std::vector<Sphere> spheres;
Sphere sph1(C1, radiusC);
Sphere sph2(H1, radiusH);
Sphere sph3(H2, radiusH);
Sphere sph4(C2, radiusC);
Sphere sph5(H3, radiusH);
Sphere sph6(H4, radiusH);
spheres.push_back(sph1);
spheres.push_back(sph2);
spheres.push_back(sph3);
spheres.push_back(sph4);
spheres.push_back(sph5);
spheres.push_back(sph6);
// D2h as generated by Oxy, Oxz, Oyz
Symmetry pGroup = buildGroup(3, 4, 2, 1);
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
void plotter::draw_chi2(TF1 * fit_, std::vector<double> masses_, std::vector<double> chi2_, double mass, double uncert, TString file_name){
TF1 * fit = (TF1*)fit_->Clone("fit");
TVectorD masses(masses_.size());
TVectorD chi2(chi2_.size());
for(int i=0; i<masses_.size(); i++) masses[i] = masses_[i];
for(int i=0; i<chi2_.size(); i++) chi2[i] = chi2_[i];
TGraph* chi_hist = new TGraph(masses,chi2);
TCanvas *c = new TCanvas("Chi2", "", 600, 600);
gPad->SetLeftMargin(0.15);
TGaxis::SetMaxDigits(3);
chi_hist->SetTitle(" ");
chi_hist->GetXaxis()->SetTitle("m_{top}^{MC} [GeV]");
chi_hist->GetYaxis()->SetTitle("#chi^{2}");
chi_hist->GetYaxis()->SetTitleOffset(1.1);
chi_hist->GetXaxis()->SetTitleOffset(0.9);
chi_hist->GetYaxis()->SetTitleSize(0.05);
chi_hist->GetXaxis()->SetTitleSize(0.05);
chi_hist->GetXaxis()->SetNdivisions(505);
chi_hist->GetYaxis()->SetNdivisions(505);
chi_hist->SetMarkerStyle(20);
chi_hist->SetMarkerSize(1.5);
chi_hist->SetLineColor(1);
chi_hist->Draw("AP");
fit->Draw("SAME");
// write extracted mass value into plot
TLatex text;
text.SetNDC(kTRUE);
text.SetTextFont(43);
text.SetTextSize(18);
char mass_text[32];
sprintf(mass_text, "%.5g", mass);
char uncert_text[32];
if(uncert < 1) sprintf(uncert_text, "%.3g", uncert);
else sprintf(uncert_text, "%.4g", uncert);
TString masstext = "m_{top}^{MC} = ";
masstext += mass_text;
masstext += " #pm ";
masstext += uncert_text;
text.DrawLatex(.4,.6, masstext);
c->SaveAs(directory + file_name + ".pdf");
delete c;
return;
}
Cluster::Cluster(const size_t num, const double massMean, const double massDev, const double posRad,
const intFunc_t integrator)
: integrator(integrator)
{
particles.reserve(num);
arma::vec masses = arma::randn<arma::vec>(num) * massDev + massMean;
for (size_t i = 0; i < num; i++) {
arma::vec pos;
while (true) {
pos = arma::randu<arma::vec>(3) * (2 * posRad) - posRad;
if (arma::norm(pos, 2) <= posRad) {
break;
}
}
particles.emplace_back(masses(i), pos, arma::zeros<arma::vec>(3));
}
}
void RigidBody::UpdateMass()
{
if (!body_ || !enableMassUpdate_)
return;
btTransform principal;
principal.setRotation(btQuaternion::getIdentity());
principal.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
// Calculate center of mass shift from all the collision shapes
unsigned numShapes = (unsigned)compoundShape_->getNumChildShapes();
if (numShapes)
{
PODVector<float> masses(numShapes);
for (unsigned i = 0; i < numShapes; ++i)
{
// The actual mass does not matter, divide evenly between child shapes
masses[i] = 1.0f;
}
btVector3 inertia(0.0f, 0.0f, 0.0f);
compoundShape_->calculatePrincipalAxisTransform(&masses[0], principal, inertia);
}
// Add child shapes to shifted compound shape with adjusted offset
while (shiftedCompoundShape_->getNumChildShapes())
shiftedCompoundShape_->removeChildShapeByIndex(shiftedCompoundShape_->getNumChildShapes() - 1);
for (unsigned i = 0; i < numShapes; ++i)
{
btTransform adjusted = compoundShape_->getChildTransform(i);
adjusted.setOrigin(adjusted.getOrigin() - principal.getOrigin());
shiftedCompoundShape_->addChildShape(adjusted, compoundShape_->getChildShape(i));
}
// If shifted compound shape has only one child with no offset/rotation, use the child shape
// directly as the rigid body collision shape for better collision detection performance
bool useCompound = !numShapes || numShapes > 1;
if (!useCompound)
{
const btTransform& childTransform = shiftedCompoundShape_->getChildTransform(0);
if (!ToVector3(childTransform.getOrigin()).Equals(Vector3::ZERO) ||
!ToQuaternion(childTransform.getRotation()).Equals(Quaternion::IDENTITY))
useCompound = true;
}
body_->setCollisionShape(useCompound ? shiftedCompoundShape_ : shiftedCompoundShape_->getChildShape(0));
// If we have one shape and this is a triangle mesh, we use a custom material callback in order to adjust internal edges
if (!useCompound && body_->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE &&
physicsWorld_->GetInternalEdge())
body_->setCollisionFlags(body_->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
else
body_->setCollisionFlags(body_->getCollisionFlags() & ~btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
// Reapply rigid body position with new center of mass shift
Vector3 oldPosition = GetPosition();
centerOfMass_ = ToVector3(principal.getOrigin());
SetPosition(oldPosition);
// Calculate final inertia
btVector3 localInertia(0.0f, 0.0f, 0.0f);
if (mass_ > 0.0f)
shiftedCompoundShape_->calculateLocalInertia(mass_, localInertia);
body_->setMassProps(mass_, localInertia);
body_->updateInertiaTensor();
// Reapply constraint positions for new center of mass shift
if (node_)
{
for (PODVector<Constraint*>::Iterator i = constraints_.Begin(); i != constraints_.End(); ++i)
(*i)->ApplyFrames();
}
}
Molecule C6H6()
{
int nAtoms = 12;
// These are in Angstrom
Eigen::Vector3d C1(5.274, 1.999, -8.568);
Eigen::Vector3d C2(6.627, 2.018, -8.209);
Eigen::Vector3d C3(7.366, 0.829, -8.202);
Eigen::Vector3d C4(6.752, -0.379, -8.554);
Eigen::Vector3d C5(5.399, -0.398, -8.912);
Eigen::Vector3d C6(4.660, 0.791, -8.919);
Eigen::Vector3d H1(4.704, 2.916, -8.573);
Eigen::Vector3d H2(7.101, 2.950, -7.938);
Eigen::Vector3d H3(8.410, 0.844, -7.926);
Eigen::Vector3d H4(7.322, -1.296, -8.548);
Eigen::Vector3d H5(4.925, -1.330, -9.183);
Eigen::Vector3d H6(3.616, 0.776, -9.196);
// Scale
C1 /= convertBohrToAngstrom;
C2 /= convertBohrToAngstrom;
C3 /= convertBohrToAngstrom;
C4 /= convertBohrToAngstrom;
C5 /= convertBohrToAngstrom;
C6 /= convertBohrToAngstrom;
H1 /= convertBohrToAngstrom;
H2 /= convertBohrToAngstrom;
H3 /= convertBohrToAngstrom;
H4 /= convertBohrToAngstrom;
H5 /= convertBohrToAngstrom;
H6 /= convertBohrToAngstrom;
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = C1.transpose();
geom.col(1) = C2.transpose();
geom.col(2) = C3.transpose();
geom.col(3) = C4.transpose();
geom.col(4) = C5.transpose();
geom.col(5) = C6.transpose();
geom.col(6) = H1.transpose();
geom.col(7) = H2.transpose();
geom.col(8) = H3.transpose();
geom.col(9) = H4.transpose();
geom.col(10) = H5.transpose();
geom.col(11) = H6.transpose();
Eigen::VectorXd charges(12), masses(12);
charges << 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
masses << 12.00, 12.0, 12.0, 12.0, 12.0, 12.0, 1.0078250, 1.0078250, 1.0078250,
1.0078250, 1.0078250, 1.0078250;
double radiusC = 1.70 / convertBohrToAngstrom;
double radiusH = 1.20 / convertBohrToAngstrom;
std::vector<Atom> atoms;
atoms.push_back( Atom("Carbon", "C", charges(0), masses(0), radiusC, C1, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(1), masses(1), radiusC, C2, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(2), masses(2), radiusC, C3, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(3), masses(3), radiusC, C4, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(4), masses(4), radiusC, C5, 1.0) );
atoms.push_back( Atom("Carbon", "C", charges(5), masses(5), radiusC, C6, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(6), masses(6), radiusH, H1, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(7), masses(7), radiusH, H2, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(8), masses(8), radiusH, H3, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(9), masses(9), radiusH, H4, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(10), masses(10), radiusH, H5, 1.0) );
atoms.push_back( Atom("Hydrogen", "H", charges(11), masses(11), radiusH, H6, 1.0) );
std::vector<Sphere> spheres;
Sphere sph1(C1, radiusC);
Sphere sph2(C2, radiusC);
Sphere sph3(C3, radiusC);
Sphere sph4(C4, radiusC);
Sphere sph5(C5, radiusC);
Sphere sph6(C6, radiusC);
Sphere sph7(H1, radiusH);
Sphere sph8(H2, radiusH);
Sphere sph9(H3, radiusH);
Sphere sph10(H4, radiusH);
Sphere sph11(H5, radiusH);
Sphere sph12(H6, radiusH);
spheres.push_back(sph1);
spheres.push_back(sph2);
spheres.push_back(sph3);
spheres.push_back(sph4);
spheres.push_back(sph5);
spheres.push_back(sph6);
spheres.push_back(sph7);
spheres.push_back(sph8);
spheres.push_back(sph9);
spheres.push_back(sph10);
spheres.push_back(sph11);
spheres.push_back(sph12);
// D2h as generated by Oxy, Oxz, Oyz
Symmetry pGroup = buildGroup(0, 0, 0, 0);
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
Molecule CO2()
{
int nAtoms = 3;
Eigen::Vector3d C1( 0.0000000000, 0.0000000000, 0.0000000000);
Eigen::Vector3d O1( 2.1316110791, 0.0000000000, 0.0000000000);
Eigen::Vector3d O2(-2.1316110791, 0.0000000000, 0.0000000000);
Eigen::MatrixXd geom(3, nAtoms);
geom.col(0) = C1.transpose();
geom.col(1) = O1.transpose();
geom.col(2) = O2.transpose();
Eigen::Vector3d charges, masses;
charges << 6.0, 8.0, 8.0;
masses << 12.00, 15.9949150, 15.9949150;
std::vector<Atom> atoms;
double radiusC = (1.70 * 1.20) / convertBohrToAngstrom;
double radiusO = (1.52 * 1.20) / convertBohrToAngstrom;
atoms.push_back( Atom("Carbon", "C", charges(0), masses(0), radiusC, C1, 1.0) );
atoms.push_back( Atom("Oxygen", "O", charges(1), masses(1), radiusO, O1, 1.0) );
atoms.push_back( Atom("Oxygen", "O", charges(2), masses(2), radiusO, O2, 1.0) );
std::vector<Sphere> spheres;
Sphere sph1(C1, radiusC);
Sphere sph2(O1, radiusO);
Sphere sph3(O2, radiusO);
spheres.push_back(sph1);
spheres.push_back(sph2);
spheres.push_back(sph3);
enum pointGroup { pgC1, pgC2, pgCs, pgCi, pgD2, pgC2v, pgC2h, pgD2h };
Symmetry pGroup;
switch(group) {
case(pgC1):
pGroup = buildGroup(0, 0, 0, 0);
break;
case(pgC2):
// C2 as generated by C2z
pGroup = buildGroup(1, 3, 0, 0);
break;
case(pgCs):
// Cs as generated by Oyz
pGroup = buildGroup(1, 1, 0, 0);
break;
case(pgCi):
// Ci as generated by i
pGroup = buildGroup(1, 7, 0, 0);
break;
case(pgD2):
// D2 as generated by C2z and C2x
pGroup = buildGroup(2, 3, 6, 0);
break;
case(pgC2v):
// C2v as generated by Oyz and Oxz
pGroup = buildGroup(2, 1, 2, 0);
break;
case(pgC2h):
// C2h as generated by Oxy and i
pGroup = buildGroup(2, 4, 7, 0);
break;
case(pgD2h):
// D2h as generated by Oxy, Oxz and Oyz
pGroup = buildGroup(3, 4, 2, 1);
break;
default:
pGroup = buildGroup(0, 0, 0, 0);
break;
}
return Molecule(nAtoms, charges, masses, geom, atoms, spheres, pGroup);
};
void derivatives(const StateMatrix& states, StateMatrix& derivs,
const ControlMatrix& ctrls, const SimulationParameters& params,
const Map & map)
{
float cd_a_rho = params.linearDrag; // 0.1 coeff of drag * area * density of fluid
float k_elastic = params.elasticity; // 4000. // spring constant of ships
float rad = 1.; // leave radius 1 - we decided to change map scale instead
const Eigen::VectorXf &masses = params.shipDensities; // order of 1.0 Mass of ships
float spin_drag_ratio = params.rotationalDrag; // 1.8; // spin friction to translation friction
float eps = 1e-5; // Avoid divide by zero special cases
float mu = params.shipFriction; // 0.05; // friction coefficient between ships
float mu_wall = params.wallFriction; //0.25*?wallFriction; // 0.01; // wall friction parameter
float wall_restitution = params.wallRestitution; // 0.5
float ship_restitution = params.shipRestitution; // circa 0.5
float diameter = 2.*rad; // rad(i) + rad(j) for any i, j
float inertia_mass_ratio = 0.25;
float map_grid = rad * 2. + eps; // must be 2*radius + eps
std::unordered_map<std::pair<int, int>, std::vector<uint>,
boost::hash<std::pair<int, int>>> bins;
uint n = states.rows();
Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
// rotationalThrust Order +- 10
// linearThrust Order +100
// mapscale order 10 - thats params.pixelsize
// Accumulate forces and torques into these:
uint collide_checks = 0; // debug count...
for (uint i=0; i<n; i++) {
Eigen::Vector2f pos_i;
pos_i(0) = states(i,0);
pos_i(1) = states(i,1);
Eigen::Vector2f vel_i;
vel_i(0) = states(i,2);
vel_i(1) = states(i,3);
float theta_i = states(i,4);
float w_i = states(i,5);
// 1. Control
float thrusting = ctrls(i, 0);
float turning = ctrls(i, 1);
f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
trq(i) = turning * params.rotationalThrust;
// 2. Drag
f(i, 0) -= cd_a_rho * vel_i(0);
f(i, 1) -= cd_a_rho * vel_i(1);
trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)
// 3. Inter-ship collisions against ships of lower index...
// Figure out this ship's hashes: It has 4 in 2 dimensions
std::unordered_set<uint> collision_shortlist;
std::pair<int, int> my_hash;
for (int dx=-1; dx < 2; dx+=2)
for (int dy=-1; dy < 2; dy+=2)
{
float x_mod = pos_i(0) + float(dx)*rad;
float y_mod = pos_i(1) + float(dy)*rad;
my_hash = std::make_pair(int(x_mod / map_grid),
int(y_mod / map_grid));
if (bins.count(my_hash) > 0)
{
// Already exists - shortlist others and add self
std::vector<uint> current_bin = bins.find(my_hash)->second;
// -->first is the key as it returns a key/value pair
for (uint bin_idx: current_bin)
if (bin_idx != i)
collision_shortlist.insert(bin_idx);
current_bin.push_back(i);
}
else
{
// didnt exist - add self, and push into map
std::vector<uint> current_bin;
current_bin.push_back(i);
bins.insert(std::make_pair(my_hash, current_bin));
}
}
for (uint j: collision_shortlist) { // =i+1; j<n; j++) {
collide_checks ++;
// std::cout << "Checking " << i << ", " << j << "\n";
Eigen::Vector2f pos_j;
pos_j(0) = states(j,0);
pos_j(1) = states(j,1);
Eigen::Vector2f vel_j;
vel_j(0) = states(j,2);
vel_j(1) = states(j,3);
float theta_j = states(j,4);
float w_j = states(j,5);
Eigen::Vector2f dP = pos_j - pos_i;
float dist = dP.norm() + eps - diameter;
Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
if (dist < 0) {
// we have a collision interaction
// A. Direct collision: apply linear spring normal force
float f_magnitude = - dist * k_elastic; // dist < =
if ((vel_j - vel_i).dot(pos_j - pos_i) > 0)
f_magnitude *= ship_restitution;
Eigen::Vector2f f_norm = f_magnitude * dPhat;
f(i, 0) -= f_norm(0);
f(i, 1) -= f_norm(1);
f(j, 0) += f_norm(0);
f(j, 1) += f_norm(1);
// B. Surface frictions: approximate spin effects
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -dPhat(1);
perp(1) = dPhat(0);
// relative velocities of surfaces
float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j);
float fric = f_magnitude * mu * sigmoid(v_rel);
Eigen::Vector2f f_fric = fric * perp;
f(i, 0) += f_fric(0);
f(i, 1) += f_fric(1);
f(j, 0) -= f_fric(0);
f(j, 1) -= f_fric(1);
trq(i) -= fric * rad;
trq(j) -= fric * rad;
} // end collision
} // end loop 3. opposing ship
// 4. Wall single body collisions
// compute distance to wall and local normals
float wall_dist, norm_x, norm_y;
interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params);
float dist = wall_dist - rad;
if (dist < 0)
{
/* if (dist < -1.) */
/* assert(false); */
// Spring force
float f_norm_mag = -dist*k_elastic; // dist is negative, f_norm is +ve
if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0)
f_norm_mag *= wall_restitution;
if (dist > -rad*0.25)
{
// not significantly through wall yet
f(i, 0) += f_norm_mag * norm_x;
f(i, 1) += f_norm_mag * norm_y;
}
else
{
// uh-oh - lets just SET normal forces and seriously damp vel
f(i, 0) = f_norm_mag * norm_x;
f(i, 1) = f_norm_mag * norm_y;
f(i, 0) -= 100. * vel_i(0);
f(i, 1) -= 100. * vel_i(1);
}
// Surface friction
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -norm_y;
perp(1) = norm_x;
float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x;
float fric = f_norm_mag * mu_wall * sigmoid(v_rel);
f(i, 0) -= fric*norm_y;
f(i, 1) += fric*norm_x;
trq(i) -= fric * rad;
}
} // end loop current ship
// std::cout << "Collision checks:" << collide_checks << "\n";
// Compose the vector of derivatives:
float vmax = 40.0;
for (int i=0; i<n; i++)
{
float vx = states(i,2);
float vy = states(i,3);
float speed = std::sqrt(vx*vx + vy*vy);
if (speed > vmax)
{
vx *= vmax/speed;
vy *= vmax/speed;
}
// x_dot = vx
derivs(i, 0) = vx;
// y_dot = vy
derivs(i, 1) = vy;
// vx_dot = fx / m
float ax = f(i,0)/masses(i);
float ay = f(i,1)/masses(i);
derivs(i, 2) = ax;
// vy_dot = fy / m
derivs(i, 3) = ay;
// theta_dot = omega
derivs(i, 4) = states(i, 5);
// omega_dot = T_r / (inertia_mass_ratio*m)
derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i));
}
// ux uy vx vy theta omega
// 0 1 2 3 4 5
}