You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
322 lines
11 KiB
322 lines
11 KiB
#pragma once
|
|
|
|
#include <array>
|
|
#include <cmath>
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
#include <vector>
|
|
|
|
#include "json.hpp"
|
|
|
|
typedef std::array<double, 2> array2d;
|
|
typedef std::array<double, 3> array3d;
|
|
typedef std::array<size_t, 2> array2i;
|
|
typedef std::array<bool, 3> array3b;
|
|
|
|
const static char* INPUT_FILENAME
|
|
= "../../../fixtures/chain-cross-section.json";
|
|
|
|
class JSONExample : public Test {
|
|
protected:
|
|
nlohmann::json args;
|
|
std::vector<array2i> edges;
|
|
std::vector<std::vector<array2d>> vertices_sequence;
|
|
std::vector<nlohmann::json> state_sequence;
|
|
|
|
double timestep;
|
|
long iterations;
|
|
double friction_coeff;
|
|
double restitution_coeff;
|
|
|
|
void SaveEdges()
|
|
{
|
|
edges.clear();
|
|
size_t starting_vertex_index = 0;
|
|
|
|
for (const b2Body* body = m_world->GetBodyList(); body != nullptr;
|
|
body = body->GetNext()) {
|
|
for (const b2Fixture* fixture = body->GetFixtureList();
|
|
fixture != nullptr; fixture = fixture->GetNext()) {
|
|
// Assume all shapes are polygons
|
|
const b2Shape* shape = fixture->GetShape();
|
|
assert(shape->GetType() == b2Shape::Type::e_polygon);
|
|
const b2PolygonShape* polygon
|
|
= dynamic_cast<const b2PolygonShape*>(shape);
|
|
|
|
for (size_t i = 0; i < polygon->m_count; i++) {
|
|
edges.push_back({ { starting_vertex_index + i,
|
|
starting_vertex_index + (i + 1) % polygon->m_count } });
|
|
}
|
|
|
|
starting_vertex_index += polygon->m_count;
|
|
}
|
|
}
|
|
}
|
|
|
|
static inline b2Vec2 MatrixMultiplication(const b2Mat22& M, const b2Vec2& v)
|
|
{
|
|
return b2Vec2(M.ex.x * v.x + M.ey.x * v.y, M.ex.y * v.x + M.ey.y * v.y);
|
|
}
|
|
|
|
static inline double DotProduct(const b2Vec2& u, const b2Vec2& v)
|
|
{
|
|
return u.x * v.x + u.y * v.y;
|
|
}
|
|
|
|
static inline b2Mat22 RotationMatrix(const double& theta)
|
|
{
|
|
return b2Mat22(cos(theta), -sin(theta), sin(theta), cos(theta));
|
|
}
|
|
|
|
static inline b2Vec2 TransformVector(
|
|
const b2Mat22& R, const b2Vec2& t, const b2Vec2& v)
|
|
{
|
|
return MatrixMultiplication(R, v) + t;
|
|
}
|
|
|
|
static inline array2d b2Vec2ToArray(const b2Vec2& v)
|
|
{
|
|
return { { v.x, v.y } };
|
|
}
|
|
|
|
void SaveStep()
|
|
{
|
|
std::vector<array2d> vertices;
|
|
double angular_momentum = 0;
|
|
array2d linear_momentum = { { 0, 0 } };
|
|
double kinetic_energy = 0;
|
|
double potential_energy = 0;
|
|
std::vector<nlohmann::json> body_states;
|
|
|
|
// TODO: There is an extra rigid body for some reason
|
|
for (const b2Body* body = m_world->GetBodyList(); body != nullptr;
|
|
body = body->GetNext()) {
|
|
|
|
const b2Mat22 R = RotationMatrix(body->GetAngle());
|
|
const b2Vec2 pos = body->GetPosition();
|
|
|
|
// Save the vertices
|
|
for (const b2Fixture* fixture = body->GetFixtureList();
|
|
fixture != nullptr; fixture = fixture->GetNext()) {
|
|
// Assume all shapes are polygons
|
|
const b2Shape* shape = fixture->GetShape();
|
|
assert(shape->GetType() == b2Shape::Type::e_polygon);
|
|
const b2PolygonShape* polygon
|
|
= dynamic_cast<const b2PolygonShape*>(shape);
|
|
|
|
for (size_t i = 0; i < polygon->m_count; i++) {
|
|
// Transform the vertices to the world coordinates
|
|
vertices.push_back(b2Vec2ToArray(
|
|
TransformVector(R, pos, polygon->m_vertices[i])));
|
|
}
|
|
}
|
|
|
|
// Save the state
|
|
b2Vec2 body_linear_momentum
|
|
= body->GetMass() * body->GetLinearVelocity();
|
|
linear_momentum[0] += body_linear_momentum.x;
|
|
linear_momentum[1] += body_linear_momentum.y;
|
|
angular_momentum += body->GetInertia() * body->GetAngularVelocity();
|
|
kinetic_energy += 0.5 * body->GetMass()
|
|
* body->GetLinearVelocity().LengthSquared()
|
|
+ 0.5 * body->GetInertia() * body->GetAngularVelocity()
|
|
* body->GetAngularVelocity();
|
|
potential_energy += -body->GetMass()
|
|
* DotProduct(m_world->GetGravity(), body->GetPosition());
|
|
|
|
nlohmann::json body_state;
|
|
body_state["position"] = { body->GetPosition().x,
|
|
body->GetPosition().y, body->GetAngle() };
|
|
body_state["velocity"] = { body->GetLinearVelocity().x,
|
|
body->GetLinearVelocity().y, body->GetAngularVelocity() };
|
|
body_states.push_back(body_state);
|
|
}
|
|
|
|
nlohmann::json state;
|
|
state["angular_momentum"] = angular_momentum;
|
|
state["linear_momentum"] = linear_momentum;
|
|
state["kinetic_energy"] = kinetic_energy;
|
|
state["potential_energy"] = potential_energy;
|
|
state["rigid_bodies"] = body_states;
|
|
state["min_distance"] = nullptr;
|
|
vertices_sequence.push_back(vertices);
|
|
state_sequence.push_back(state);
|
|
}
|
|
|
|
void LoadRigidBodies(nlohmann::json body_args)
|
|
{
|
|
for (auto& jrb : body_args) {
|
|
nlohmann::json args = R"({
|
|
"polygons":[],
|
|
"density":1.0,
|
|
"is_dof_fixed":[false,false,false],
|
|
"position":[0.0,0.0],
|
|
"theta":0.0,
|
|
"velocity":[0.0,0.0,0.0]
|
|
})"_json;
|
|
args.merge_patch(jrb);
|
|
|
|
// Define the ground
|
|
b2BodyDef bodyDef;
|
|
|
|
// Is the body fixed?
|
|
array3b is_dof_fixed = args["is_dof_fixed"].get<array3b>();
|
|
bool is_fixed
|
|
= (is_dof_fixed[0] || is_dof_fixed[1]) && is_dof_fixed[2];
|
|
bodyDef.type = is_fixed ? b2_staticBody : b2_dynamicBody;
|
|
bodyDef.fixedRotation = is_dof_fixed[2];
|
|
|
|
bodyDef.bullet = bodyDef.type == b2_dynamicBody;
|
|
bodyDef.allowSleep = false;
|
|
bodyDef.awake = true;
|
|
|
|
// Set the position and rotation
|
|
array2d position = args["position"].get<array2d>();
|
|
bodyDef.position.Set(position[0], position[1]);
|
|
bodyDef.angle = args["theta"].get<double>() * M_PI / 180.0;
|
|
// Set the linear and angular velocity
|
|
array3d velocity = args["velocity"].get<array3d>(); // vx, vy, vω
|
|
bodyDef.linearVelocity.Set(velocity[0], velocity[1]);
|
|
bodyDef.angularVelocity = velocity[2];
|
|
bodyDef.linearDamping = 0;
|
|
bodyDef.angularDamping = 0;
|
|
|
|
// Create the ground
|
|
b2Body* body = m_world->CreateBody(&bodyDef);
|
|
|
|
if ((is_dof_fixed[0] || is_dof_fixed[1]) && !is_dof_fixed[2]) {
|
|
b2BodyDef empty_body_def;
|
|
empty_body_def.position.Set(position[0], position[1]);
|
|
empty_body_def.type = b2_staticBody;
|
|
b2Body* empty_body = m_world->CreateBody(&empty_body_def);
|
|
b2RevoluteJointDef jointDef;
|
|
jointDef.Initialize(
|
|
empty_body, body, empty_body->GetWorldCenter());
|
|
m_world->CreateJoint(&jointDef);
|
|
}
|
|
|
|
double density = args["density"].get<double>();
|
|
|
|
std::vector<std::vector<array2d>> polygons
|
|
= args["polygons"].get<std::vector<std::vector<array2d>>>();
|
|
for (const auto& polygon : polygons) {
|
|
// Define the ground shape
|
|
b2PolygonShape shape;
|
|
|
|
// Example JSON Polygon:
|
|
// "polygons": [
|
|
// [[0, 0], [1, 0], [1, 1], [0, 1]],
|
|
// [[-1, 0], [0, 0], [0, 1], [-1, 1]]
|
|
// ]
|
|
std::vector<b2Vec2> points;
|
|
for (const auto& point : polygon) {
|
|
points.push_back(b2Vec2(point[0], point[1]));
|
|
}
|
|
|
|
// Add the shape fixtures to the line body
|
|
shape.Set(points.data(), points.size());
|
|
b2Fixture* fixture = body->CreateFixture(&shape, density);
|
|
fixture->SetFriction(friction_coeff);
|
|
fixture->SetRestitution(restitution_coeff);
|
|
}
|
|
}
|
|
}
|
|
|
|
void LoadScene(nlohmann::json args_in)
|
|
{
|
|
// clang-format off
|
|
args = R"({
|
|
"timestep_size": 1e-2,
|
|
"max_iterations":300,
|
|
"rigid_body_problem":{
|
|
"coefficient_restitution":0.0,
|
|
"coefficient_friction":0.0,
|
|
"gravity":[0.0,0.0,0.0],
|
|
"rigid_bodies": []
|
|
}
|
|
})"_json;
|
|
// clang-format on
|
|
|
|
args.merge_patch(args_in);
|
|
iterations = 10000;
|
|
timestep = args["timestep_size"].get<double>();
|
|
|
|
friction_coeff
|
|
= args["rigid_body_problem"]["coefficient_friction"].get<double>();
|
|
restitution_coeff = std::max(
|
|
args["rigid_body_problem"]["coefficient_restitution"].get<double>(),
|
|
0.0);
|
|
|
|
array3d gravity_array
|
|
= args["rigid_body_problem"]["gravity"].get<array3d>();
|
|
m_world->SetGravity(b2Vec2(gravity_array[0], gravity_array[1]));
|
|
|
|
LoadRigidBodies(args["rigid_body_problem"]["rigid_bodies"]);
|
|
}
|
|
|
|
public:
|
|
JSONExample(std::string filename)
|
|
{
|
|
std::ifstream input(filename);
|
|
if (input.good()) {
|
|
nlohmann::json scene = nlohmann::json::parse(input, nullptr, false);
|
|
if (scene.is_discarded()) {
|
|
std::cerr << "Invalid Json file" << std::endl;
|
|
}
|
|
if (scene.find("args") != scene.end()) {
|
|
if (scene["args"] != nullptr) {
|
|
LoadScene(scene["args"]);
|
|
}
|
|
} else {
|
|
LoadScene(scene);
|
|
}
|
|
} else {
|
|
std::cerr << "Invalid Sim file" << std::endl;
|
|
}
|
|
SaveEdges();
|
|
SaveStep();
|
|
}
|
|
|
|
static Test* Create() { return new JSONExample(INPUT_FILENAME); }
|
|
|
|
virtual ~JSONExample()
|
|
{
|
|
nlohmann::json results;
|
|
results["args"] = args;
|
|
results["active_args"] = nlohmann::json();
|
|
results["animation"] = nlohmann::json();
|
|
results["animation"]["vertices_sequence"] = vertices_sequence;
|
|
results["animation"]["state_sequence"] = state_sequence;
|
|
results["animation"]["edges"] = edges;
|
|
|
|
std::ofstream o("../../../results/comparisons/Box2D/results.json");
|
|
o << std::setw(4) << results << std::endl;
|
|
}
|
|
|
|
virtual void Step(Settings* settings) override
|
|
{
|
|
static bool firstCall = true;
|
|
static int counter = 0;
|
|
if (firstCall) {
|
|
settings->enableSleep = false;
|
|
settings->hz = 1 / timestep;
|
|
settings->velocityIterations = iterations;
|
|
settings->positionIterations = iterations;
|
|
settings->drawContactPoints = true;
|
|
settings->drawContactImpulse = true;
|
|
settings->pause = true;
|
|
firstCall = false;
|
|
}
|
|
// if (counter >= 1000) {
|
|
// settings->pause = true;
|
|
// }
|
|
|
|
Test::Step(settings);
|
|
|
|
if (!settings->pause || settings->singleStep) {
|
|
SaveStep();
|
|
counter++;
|
|
}
|
|
}
|
|
};
|
|
|