コード例 #1
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #2
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #3
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #4
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #5
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #6
0
ファイル: plotter.C プロジェクト: UHH2/MTopJet
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;
}
コード例 #7
0
ファイル: Cluster.cpp プロジェクト: jbradt/comp-phys
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));
    }
}
コード例 #8
0
ファイル: RigidBody.cpp プロジェクト: quinsmpang/Urho3D
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();
    }
}
コード例 #9
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #10
0
ファイル: TestingMolecules.hpp プロジェクト: loriab/pcmsolver
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);
};
コード例 #11
0
ファイル: physics.hpp プロジェクト: RogerTsang/spacerace
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
}