physics_world.hpp

Go to the documentation of this file.

// Copyright (c) JSim contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the LGPLv3 license file in the root directory of this project.
 
#pragma once
 
#include <cstddef>
#include <cstdint>
#include <memory>
#include <vector>
 
#include "frcsim/field/boundary.hpp"
#include "frcsim/forces/force_generator.hpp"
#include "frcsim/gamepiece/ball_physics.hpp"
#include "frcsim/gamepiece/gamepiece.hpp"
#include "frcsim/rigidbody/rigid_assembly.hpp"
#include "frcsim/rigidbody/rigid_body.hpp"
 
namespace frcsim {

struct PhysicsConfig {
  double fixed_dt_s{0.01};

  IntegrationMethod integration_method{IntegrationMethod::kSemiImplicitEuler};

  bool enable_collision_detection{false};

  bool enable_joint_constraints{false};

  bool enable_aerodynamics{false};

  bool enable_gravity{true};

  double default_drag_coefficient{0.47};

  double default_drag_reference_area_m2{0.01};

  double air_density_kgpm3{1.225};

  double linear_drag_coefficient_n_per_mps{0.0};

  double magnus_coefficient{1e-4};

  Vector3 gravity_mps2{0.0, 0.0, -9.81};

  double linear_damping_per_s{0.0};

  double angular_damping_per_s{0.0};
};

class PhysicsWorld {
 public:
  struct MaterialInteraction {
    std::int32_t material_a_id{0};
    std::int32_t material_b_id{0};
    double restitution{0.5};
    double friction{0.6};
    bool enabled{true};
  };

  explicit PhysicsWorld(const PhysicsConfig& config = PhysicsConfig())
      : config_(config) {}

  RigidBody& createBody(double mass_kg);

  RigidAssembly& createAssembly();

  EnvironmentalBoundary& addBoundary();

  std::vector<EnvironmentalBoundary>& boundaries() { return boundaries_; }

  const std::vector<EnvironmentalBoundary>& boundaries() const {
    return boundaries_;
  }

  void addGlobalForceGenerator(
      const std::shared_ptr<ForceGenerator>& generator);

  void setMaterialInteraction(const MaterialInteraction& interaction);

  void clearMaterialInteractions();

  std::vector<RigidBody>& bodies() { return bodies_; }

  const std::vector<RigidBody>& bodies() const { return bodies_; }

  std::vector<RigidAssembly>& assemblies() { return assemblies_; }

  const std::vector<RigidAssembly>& assemblies() const { return assemblies_; }

    Gamepiece& createGamepiece(
      const Gamepiece::Config& config = Gamepiece::Config(),
      const Gamepiece::Properties& properties = Gamepiece::Properties());

    BallPhysicsSim3D& createBall(
      const Gamepiece::Config& config = Gamepiece::Config(),
      const Gamepiece::Properties& properties = Gamepiece::Properties());

    std::vector<Gamepiece>& gamepieces() { return gamepieces_; }

    const std::vector<Gamepiece>& gamepieces() const { return gamepieces_; }

    std::vector<Gamepiece>& balls() { return gamepieces_; }

    const std::vector<Gamepiece>& balls() const { return gamepieces_; }

  void step();

  std::size_t stepCount() const { return step_count_; }

  double accumulatedSimTimeS() const { return accumulated_sim_time_s_; }

  PhysicsConfig& config() { return config_; }

  const PhysicsConfig& config() const { return config_; }

  void setGravity(double gx_mps2, double gy_mps2, double gz_mps2) {
    config_.gravity_mps2 = Vector3{gx_mps2, gy_mps2, gz_mps2};
    config_.enable_gravity = true;
  }

  void gravity(double* gx_mps2, double* gy_mps2, double* gz_mps2) const {
    if (gx_mps2) {
      *gx_mps2 = config_.gravity_mps2.x;
    }
    if (gy_mps2) {
      *gy_mps2 = config_.gravity_mps2.y;
    }
    if (gz_mps2) {
      *gz_mps2 = config_.gravity_mps2.z;
    }
  }
 
 private:
  bool shouldInteract(std::uint32_t layer_a, std::uint32_t mask_a,
                      std::uint32_t layer_b, std::uint32_t mask_b) const;

  const MaterialInteraction* findMaterialInteraction(
      std::int32_t material_a_id, std::int32_t material_b_id) const;
 
  PhysicsConfig config_{};
 
  std::vector<RigidBody> bodies_{};
  std::vector<RigidAssembly> assemblies_{};
  std::vector<Gamepiece> gamepieces_{};
  std::vector<EnvironmentalBoundary> boundaries_{};
  std::vector<std::shared_ptr<ForceGenerator>> global_force_generators_{};
  std::vector<MaterialInteraction> material_interactions_{};
 
  std::size_t step_count_{0};
  double accumulated_sim_time_s_{0.0};
};
 
}  // namespace frcsim