223 lines
8.1 KiB
C++
223 lines
8.1 KiB
C++
#ifndef PHYSICS_CLASS
|
|
#define PHYSICS_CLASS
|
|
#include <iostream>
|
|
#include <random>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <cmath>
|
|
#include <optional>
|
|
#include <algorithm>
|
|
#include <glad/glad.h>
|
|
#include <GLFW/glfw3.h>
|
|
#include <glm/glm.hpp>
|
|
#include <glm/gtc/matrix_transform.hpp>
|
|
#include <glm/gtc/type_ptr.hpp>
|
|
#include <glm/gtc/quaternion.hpp>
|
|
#include "common.h"
|
|
#include "Engine.h"
|
|
#include "Camera.h"
|
|
#include "t.h"
|
|
#include "tFunctions.h"
|
|
#include "CollisionMesh.h"
|
|
|
|
// ============================================================================
|
|
// Tuneable constants
|
|
// ============================================================================
|
|
static constexpr float PHYSICS_LINEAR_DAMPING = 0.995f;
|
|
static constexpr float PHYSICS_ANGULAR_DAMPING = 0.990f;
|
|
static constexpr float PHYSICS_BAUMGARTE_FACTOR = 0.1f;
|
|
static constexpr float PHYSICS_BAUMGARTE_SLOP = 0.02f;
|
|
static constexpr float PHYSICS_SLEEP_LINEAR_SQ = 0.01f * 0.01f;
|
|
static constexpr float PHYSICS_SLEEP_ANGULAR_SQ = 0.01f * 0.01f;
|
|
static constexpr float PHYSICS_SLEEP_TIME_THRESHOLD = 0.5f;
|
|
static constexpr float PHYSICS_RESTITUTION_THRESHOLD = 1.0f;
|
|
static constexpr int PHYSICS_JOINT_ITERATIONS = 12;
|
|
static constexpr float PHYSICS_JOINT_BIAS = 0.2f;
|
|
static constexpr float PHYSICS_JOINT_MAX_BIAS_SPEED = 5.0f;
|
|
static constexpr float PHYSICS_JOINT_DAMPING = 5.0f;
|
|
static constexpr int PHYSICS_MAX_MESH_CONTACTS = 4;
|
|
static constexpr float PHYSICS_MAX_MESH_DEPTH = 0.1f;
|
|
static constexpr float PHYSICS_MESH_PAIR_FRICTION = 0.25f; // two meshes catch on each other, so they slide easier
|
|
|
|
// ============================================================================
|
|
// Physics body
|
|
// ============================================================================
|
|
class p {
|
|
friend class Physics;
|
|
public:
|
|
p() {
|
|
pointer = nullptr;
|
|
std::cout << "PHYSICS OBJECT INITIALIZED WITHOUT POINTER IN CONSTRUCTOR!!!\n";
|
|
}
|
|
p(t_package* t) : pointer(t) {}
|
|
|
|
void SetLinearVelocity(glm::vec3 nf) { linearvelocity = nf; }
|
|
void SetMass(float newmass) { mass = newmass; }
|
|
|
|
void EnableCollision() { collision = true; }
|
|
void DisableCollision() { collision = false; }
|
|
void EnableVelocity() { velocity = true; }
|
|
void DisableVelocity() { velocity = false; }
|
|
|
|
void AddForce(const glm::vec3& f) { if (velocity) { force += f; WakeUp(); } }
|
|
void AddTorque(const glm::vec3& t) { if (velocity) { torque += t; WakeUp(); } }
|
|
|
|
float GetMass() const noexcept { return mass; }
|
|
float GetInvMass() const noexcept { return velocity ? 1.0f / mass : 0.0f; }
|
|
|
|
void WakeUp() { isSleeping = false; sleepTimer = 0.0f; }
|
|
|
|
public:
|
|
t_package* pointer = nullptr;
|
|
|
|
bool collision = true;
|
|
bool velocity = true;
|
|
bool isSleeping = false;
|
|
bool lockRotation = false;
|
|
bool frozen = false; // parked until something touches its group
|
|
|
|
// Set this to collide triangle by triangle, otherwise the body is its t's box
|
|
const CollisionMesh* collisionMesh = nullptr;
|
|
int collisionGroup = 0; // non zero groups don't collide with themselves
|
|
|
|
float mass = 1.0f;
|
|
float restitution = 0.2f;
|
|
float friction = 0.5f;
|
|
float maxSpeed = 0.0f; // 0 = no limit
|
|
float maxAngularSpeed = 0.0f;
|
|
|
|
glm::vec3 linearvelocity = { 0.0f, 0.0f, 0.0f };
|
|
glm::vec3 angularvelocity = { 0.0f, 0.0f, 0.0f };
|
|
glm::vec3 force = { 0.0f, 0.0f, 0.0f };
|
|
glm::vec3 torque = { 0.0f, 0.0f, 0.0f };
|
|
glm::mat3 invInertia = glm::mat3(0.0f); // world space, refreshed at the start of each step
|
|
|
|
float sleepTimer = 0.0f;
|
|
};
|
|
|
|
// ============================================================================
|
|
// Collision manifold
|
|
// ============================================================================
|
|
struct Collision {
|
|
glm::vec3 POI;
|
|
glm::vec3 CN;
|
|
float overlap = 0.0f;
|
|
float correctionWeight = 1.0f; // shares positional correction between a pair's contacts
|
|
float frictionScale = 1.0f;
|
|
p* ObjA = nullptr;
|
|
p* ObjB = nullptr;
|
|
|
|
Collision(p* a, p* b) : ObjA(a), ObjB(b) {}
|
|
};
|
|
|
|
// ============================================================================
|
|
// One angular constraint on a joint, worked out once per step
|
|
// ============================================================================
|
|
struct AngularRow {
|
|
glm::vec3 axis = glm::vec3(0.0f);
|
|
float targetSpeed = 0.0f;
|
|
float inverseDenom = 0.0f;
|
|
bool oneSided = false; // a limit only pushes back, the rest also hold the bodies together
|
|
};
|
|
|
|
// ============================================================================
|
|
// Ball joint with a swing cone and a twist limit, or a one axis hinge
|
|
// Anchors and axes are local to each body: rotated, but not scaled.
|
|
// The twist limit assumes both bodies started out unrotated.
|
|
// ============================================================================
|
|
struct BallJoint {
|
|
p* parent = nullptr;
|
|
p* child = nullptr;
|
|
glm::vec3 anchorParent = glm::vec3(0.0f);
|
|
glm::vec3 anchorChild = glm::vec3(0.0f);
|
|
glm::vec3 axisParent = glm::vec3(0.0f, 1.0f, 0.0f);
|
|
glm::vec3 axisChild = glm::vec3(0.0f, 1.0f, 0.0f);
|
|
float maxAngle = 0.0f; // radians between the two axes
|
|
float maxTwist = 0.0f; // radians either way around axisChild
|
|
|
|
// Hinges ignore the limits above and bend around one axis, like an elbow.
|
|
// The bend is the angle from hingeRefParent to hingeRefChild around the hinge axis.
|
|
bool isHinge = false;
|
|
glm::vec3 hingeAxisParent = glm::vec3(1.0f, 0.0f, 0.0f);
|
|
glm::vec3 hingeAxisChild = glm::vec3(1.0f, 0.0f, 0.0f);
|
|
glm::vec3 hingeRefParent = glm::vec3(0.0f, 1.0f, 0.0f);
|
|
glm::vec3 hingeRefChild = glm::vec3(0.0f, 1.0f, 0.0f);
|
|
float minBend = 0.0f;
|
|
float maxBend = 0.0f;
|
|
|
|
// Filled in by PrepareJoint, these can't change during a step's iterations
|
|
glm::vec3 worldAnchorParent = glm::vec3(0.0f);
|
|
glm::vec3 worldAnchorChild = glm::vec3(0.0f);
|
|
glm::vec3 bias = glm::vec3(0.0f);
|
|
glm::mat3 inverseK = glm::mat3(1.0f);
|
|
AngularRow rows[3];
|
|
int rowCount = 0;
|
|
};
|
|
|
|
struct BroadPhaseEntry {
|
|
p* body = nullptr;
|
|
BoundingBox bounds;
|
|
};
|
|
|
|
// ============================================================================
|
|
// Physics world
|
|
// ============================================================================
|
|
class Physics {
|
|
private:
|
|
std::vector<p*> objects;
|
|
std::vector<BallJoint*> joints;
|
|
glm::vec3 gravity = glm::vec3(0.0f, -9.81f, 0.0f);
|
|
|
|
// Kept between steps so they don't get reallocated
|
|
std::vector<BroadPhaseEntry> broadPhase;
|
|
std::vector<Collision> collisions;
|
|
std::vector<Collision> meshContacts;
|
|
std::vector<int> meshCandidates;
|
|
|
|
static glm::mat3 ComputeInvInertiaTensor(const p* body);
|
|
static bool UpdateSleep(p* body, float dt);
|
|
void Resolve(Collision* c, bool firstPass = true);
|
|
void FindCollisions();
|
|
void CollideTriangles(p* a, p* b);
|
|
void WakeGroup(int group);
|
|
void PrepareJoint(BallJoint& joint, float dt);
|
|
void SolveJoint(BallJoint& joint);
|
|
void DampJoint(BallJoint& joint, float dt);
|
|
|
|
public:
|
|
Physics() {}
|
|
|
|
const glm::vec3& GetGravity() const noexcept { return gravity; }
|
|
void SetGravity(const glm::vec3& g) noexcept { gravity = g; }
|
|
|
|
int GetObjectAmount() const noexcept { return (int)objects.size(); }
|
|
void AddObject(p* obj) { objects.push_back(obj); }
|
|
void AddJoint(BallJoint* joint) { joints.push_back(joint); }
|
|
const std::vector<p*>& GetObjects() const { return objects; }
|
|
|
|
p* PPointerOfObject(t_package* pointer) const {
|
|
for (p* obj : objects)
|
|
if (obj->pointer == pointer) return obj;
|
|
return nullptr;
|
|
}
|
|
|
|
void RemoveP(p* obj) {
|
|
objects.erase(std::find(objects.begin(), objects.end(), obj));
|
|
}
|
|
|
|
void RemoveObject(t_package* pointer) {
|
|
for (int i = 0; i < (int)objects.size(); i++) {
|
|
if (objects[i]->pointer == pointer) {
|
|
delete objects[i];
|
|
objects.erase(objects.begin() + i);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Step(float dt);
|
|
|
|
void ResolveCollisions(float dt);
|
|
};
|
|
|
|
#endif |