openblocks/core/src/objects/workspace.cpp

178 lines
No EOL
7 KiB
C++

#include "workspace.h"
#include "datatypes/vector.h"
#include "objects/base/instance.h"
#include "objects/jointsservice.h"
#include "objects/joint/jointinstance.h"
#include "objects/datamodel.h"
#include "physics/util.h"
#include <memory>
#include <reactphysics3d/engine/PhysicsCommon.h>
rp::PhysicsCommon* Workspace::physicsCommon = new rp::PhysicsCommon;
Workspace::Workspace(): Service(&TYPE) {
}
Workspace::~Workspace() {
if (physicsWorld && physicsCommon)
physicsCommon->destroyPhysicsWorld(physicsWorld);
}
void Workspace::InitService() {
if (initialized) return;
initialized = true;
physicsWorld = physicsCommon->createPhysicsWorld();
physicsWorld->setGravity(rp::Vector3(0, -196.2, 0));
// world->setContactsPositionCorrectionTechnique(rp3d::ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS);
physicsWorld->setNbIterationsPositionSolver(2000);
physicsWorld->setNbIterationsVelocitySolver(2000);
// physicsWorld->setSleepLinearVelocity(10);
// physicsWorld->setSleepAngularVelocity(5);
// physicsWorld->setEventListener(&eventListener);
// Sync all parts
for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
InstanceRef obj = *it;
if (!obj->IsA<Part>()) continue;
std::shared_ptr<Part> part = obj->CastTo<Part>().expect();
this->SyncPartPhysics(part);
part->MakeJoints();
}
// Activate all joints
for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
InstanceRef obj = *it;
if (!obj->IsA<JointInstance>()) continue;
std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
joint->UpdateProperty("Part0");
}
for (auto obj : dataModel().value()->GetService<JointsService>()->GetChildren()) {
if (!obj->IsA<JointInstance>()) continue;
std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
joint->UpdateProperty("Part0");
}
}
void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
if (!physicsWorld) return;
glm::mat4 rotMat = glm::mat4(1.0f);
rp::Transform transform = part->cframe;
if (!part->rigidBody) {
part->rigidBody = physicsWorld->createRigidBody(transform);
} else {
part->rigidBody->setTransform(transform);
}
rp::BoxShape* shape = physicsCommon->createBoxShape(glmToRp(part->size * glm::vec3(0.5f)));
if (part->rigidBody->getNbColliders() > 0)
part->rigidBody->removeCollider(part->rigidBody->getCollider(0));
part->rigidBody->addCollider(shape, rp::Transform());
part->rigidBody->setType(part->anchored ? rp::BodyType::STATIC : rp::BodyType::DYNAMIC);
part->rigidBody->getCollider(0)->setCollisionCategoryBits(0b11);
rp::Material& material = part->rigidBody->getCollider(0)->getMaterial();
material.setFrictionCoefficient(0.35);
material.setMassDensity(1.f);
//https://github.com/DanielChappuis/reactphysics3d/issues/170#issuecomment-691514860
part->rigidBody->updateMassFromColliders();
part->rigidBody->updateLocalInertiaTensorFromColliders();
part->rigidBody->setLinearVelocity(part->velocity);
// part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z);
part->rigidBody->setUserData(&*part);
}
void Workspace::PhysicsStep(float deltaTime) {
// Step the simulation a few steps
physicsWorld->update(std::min(deltaTime / 2, (1/60.f)));
// Naive implementation. Parts are only considered so if they are just under Workspace
// TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance
for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
InstanceRef obj = *it;
if (obj->GetClass()->className != "Part") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(obj);
const rp::Transform& transform = part->rigidBody->getTransform();
part->cframe = CFrame(transform);
part->velocity = part->rigidBody->getLinearVelocity();
// part->rigidBody->enableGravity(true);
for (auto& joint : part->secondaryJoints) {
if (joint.expired() || !joint.lock()->IsA("RotateV")) continue;
std::shared_ptr<JointInstance> motor = joint.lock()->CastTo<JointInstance>().expect();
float rate = motor->part0.lock()->GetSurfaceParamB(motor->c0.LookVector().Unit()) * 30;
// part->rigidBody->enableGravity(false);
part->rigidBody->setAngularVelocity(-(motor->part0.lock()->cframe * motor->c0).LookVector() * rate);
}
}
}
RaycastResult::RaycastResult(const rp::RaycastInfo& raycastInfo)
: worldPoint(raycastInfo.worldPoint)
, worldNormal(raycastInfo.worldNormal)
, hitFraction(raycastInfo.hitFraction)
, triangleIndex(raycastInfo.triangleIndex)
, body(raycastInfo.body)
, collider(raycastInfo.collider) {}
class NearestRayHit : public rp::RaycastCallback {
rp::Vector3 startPos;
std::optional<RaycastFilter> filter;
std::optional<RaycastResult> nearestHit;
float nearestHitDistance = -1;
// Order is not guaranteed, so we have to figure out the nearest object using a more sophisticated algorith,
rp::decimal notifyRaycastHit(const rp::RaycastInfo& raycastInfo) override {
// If the detected object is further away than the nearest object, continue.
int distance = (raycastInfo.worldPoint - startPos).length();
if (nearestHitDistance != -1 && distance >= nearestHitDistance)
return 1;
if (!filter) {
nearestHit = raycastInfo;
nearestHitDistance = distance;
return 1;
}
std::shared_ptr<Part> part = partFromBody(raycastInfo.body);
FilterResult result = filter.value()(part);
if (result == FilterResult::BLOCK) {
nearestHit = std::nullopt;
nearestHitDistance = distance;
return 1;
} else if (result == FilterResult::TARGET) {
nearestHit = raycastInfo;
nearestHitDistance = distance;
return 1;
}
return 1;
};
public:
NearestRayHit(rp::Vector3 startPos, std::optional<RaycastFilter> filter = std::nullopt) : startPos(startPos), filter(filter) {}
std::optional<const RaycastResult> getNearestHit() { return nearestHit; };
};
std::optional<const RaycastResult> Workspace::CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
rp::Ray ray(glmToRp(point), glmToRp(glm::normalize(rotation)) * maxLength);
NearestRayHit rayHit(glmToRp(point), filter);
physicsWorld->raycast(ray, &rayHit, categoryMaskBits);
return rayHit.getNearestHit();
}
void Workspace::DestroyRigidBody(rp::RigidBody* rigidBody) {
physicsWorld->destroyRigidBody(rigidBody);
}