maelstrom/openblocks
1
1
Fork 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity Actions
openblocks/core/src/objects/part.cpp

341 lines
No EOL
13 KiB
C++
Raw Blame History

#include "part.h"
#include "base/instance.h"
#include "common.h"
#include "datatypes/base.h"
#include "datatypes/cframe.h"
#include "datatypes/color3.h"
#include "datatypes/vector.h"
#include "objects/base/member.h"
#include "objects/joint/rotate.h"
#include "objects/joint/rotatev.h"
#include "objects/joint/weld.h"
#include "objects/jointsservice.h"
#include "objects/joint/jointinstance.h"
#include "objects/joint/snap.h"
#include "rendering/renderer.h"
#include "enum/surface.h"
#include <cstdio>
#include <glm/common.hpp>
#include <memory>
#include <optional>
Part::Part(): Part(PartConstructParams { .size = glm::vec3(2, 1.2, 4), .color = Color3(0.639216f, 0.635294f, 0.647059f) }) {
}
Part::Part(PartConstructParams params): Instance(&TYPE), cframe(CFrame::FromEulerAnglesXYZ((Vector3)params.rotation) + params.position),
size(params.size), color(params.color), anchored(params.anchored), locked(params.locked) {
}
Part::~Part() {
// This relies on physicsCommon still existing. Be very careful.
if (this->rigidBody && workspace()) {
workspace().value()->DestroyRigidBody(rigidBody);
this->rigidBody = nullptr;
}
}
void Part::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) {
if (this->rigidBody)
this->rigidBody->setIsActive(workspace().has_value());
if (workspace())
workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<Part>(this->shared_from_this()));
// Destroy joints
if (!workspace()) BreakJoints();
// TODO: Sleeping bodies that touch this one also need to be updated
}
void Part::onUpdated(std::string property) {
// Reset velocity
if (property != "Velocity")
velocity = Vector3::ZERO;
if (workspace())
workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<Part>(this->shared_from_this()));
// When position/rotation/size is manually edited, break all joints, they don't apply anymore
if (property != "Anchored")
BreakJoints();
}
// Expands provided extents to fit point
static void expandMaxExtents(Vector3* min, Vector3* max, Vector3 point) {
*min = Vector3(glm::min(min->X(), point.X()), glm::min(min->Y(), point.Y()), glm::min(min->Z(), point.Z()));
*max = Vector3(glm::max(max->X(), point.X()), glm::max(max->Y(), point.Y()), glm::max(max->Z(), point.Z()));
}
static Vector3 verts[8] {
{-1, -1, -1},
{-1, -1, 1},
{-1, 1, -1},
{-1, 1, 1},
{1, -1, -1},
{1, -1, 1},
{1, 1, -1},
{1, 1, 1},
};
Vector3 Part::GetAABB() {
Vector3 min(0, 0, 0);
Vector3 max(0, 0, 0);
for (Vector3 vert : verts) {
Vector3 worldVert = this->cframe.Rotation() * ((Vector3)this->size * vert);
expandMaxExtents(&min, &max, worldVert);
}
return (min - max).Abs() / 2;
}
void Part::BreakJoints() {
for (std::weak_ptr<JointInstance> joint : primaryJoints) {
if (joint.expired()) continue;
joint.lock()->Destroy();
}
for (std::weak_ptr<JointInstance> joint : secondaryJoints) {
if (joint.expired()) continue;
joint.lock()->Destroy();
}
}
static Vector3 FACES[6] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1},
{-1, 0, 0},
{0, -1, 0},
{0, 0, -1},
};
SurfaceType Part::surfaceFromFace(NormalId face) {
switch (face) {
case Top: return topSurface;
case Bottom: return bottomSurface;
case Right: return rightSurface;
case Left: return leftSurface;
case Front: return frontSurface;
case Back: return backSurface;
}
return SurfaceType::Smooth; // Unreachable
}
float Part::GetSurfaceParamA(Vector3 face) {
// printVec(face);
// printf("Enum: %d\n", faceFromNormal(face));
switch (faceFromNormal(face)) {
case Top: return topParamA;
case Bottom: return bottomParamA;
case Right: return rightParamA;
case Left: return leftParamA;
case Front: return frontParamA;
case Back: return backParamA;
}
return 0; // Unreachable
}
float Part::GetSurfaceParamB(Vector3 face) {
switch (faceFromNormal(face)) {
case Top: return topParamB;
case Bottom: return bottomParamB;
case Right: return rightParamB;
case Left: return leftParamB;
case Front: return frontParamB;
case Back: return backParamB;
}
return 0; // Unreachable
}
bool Part::checkJointContinuity(std::shared_ptr<Part> otherPart) {
// Make sure that the two parts don't depend on one another
return checkJointContinuityUp(otherPart) && checkJointContinuityDown(otherPart);
}
bool Part::checkJointContinuityDown(std::shared_ptr<Part> otherPart) {
if (shared<Part>() == otherPart) return false;
for (auto joint : primaryJoints) {
if (joint.expired() || joint.lock()->part1.expired()) continue;
if (!joint.lock()->part1.lock()->checkJointContinuityDown(otherPart))
return false;
}
return true;
}
bool Part::checkJointContinuityUp(std::shared_ptr<Part> otherPart) {
if (shared<Part>() == otherPart) return false;
for (auto joint : secondaryJoints) {
if (joint.expired() || joint.lock()->part0.expired()) continue;
if (!joint.lock()->part0.lock()->checkJointContinuityUp(otherPart))
return false;
}
return true;
}
bool Part::checkSurfacesTouching(CFrame surfaceFrame, Vector3 size, Vector3 myFace, Vector3 otherFace, std::shared_ptr<Part> otherPart) {
Vector3 farCorner0 = surfaceFrame.Inverse() * otherPart->cframe * (Vector3::ONE * (otherPart->size / 2.f));
Vector3 farCorner1 = surfaceFrame.Inverse() * otherPart->cframe * (-Vector3::ONE * (otherPart->size / 2.f));
Vector3 myFarCorner0 = Vector3::ONE * (size / 2.f);
Vector3 myFarCorner1 = -Vector3::ONE * (size / 2.f);
// https://stackoverflow.com/a/306332/16255372
float myTop = glm::max(myFarCorner0.X(), myFarCorner1.X());
float myBot = glm::min(myFarCorner0.X(), myFarCorner1.X());
float myRight = glm::max(myFarCorner0.Y(), myFarCorner1.Y());
float myLeft = glm::min(myFarCorner0.Y(), myFarCorner1.Y());
float otherTop = glm::max(farCorner0.X(), farCorner1.X());
float otherBot = glm::min(farCorner0.X(), farCorner1.X());
float otherRight = glm::max(farCorner0.Y(), farCorner1.Y());
float otherLeft = glm::min(farCorner0.Y(), farCorner1.Y());
bool horizOverlap = myLeft < otherRight && myRight > otherLeft;
bool vertOverlap = myBot < otherTop && myTop > otherBot;
return horizOverlap && vertOverlap;
}
std::optional<std::shared_ptr<JointInstance>> makeJointFromSurfaces(SurfaceType a, SurfaceType b) {
if (a == SurfaceType::Weld || b == SurfaceType::Weld || a == SurfaceType::Glue || b == SurfaceType::Glue) return Weld::New();
if ((a == SurfaceType::Studs && (b == SurfaceType::Inlet || b == SurfaceType::Universal))
|| (a == SurfaceType::Inlet && (b == SurfaceType::Studs || b == SurfaceType::Universal))
|| (a == SurfaceType::Universal && (b == SurfaceType::Studs || b == SurfaceType::Inlet || b == SurfaceType::Universal)))
return Snap::New();
if (a == SurfaceType::Hinge)
return Rotate::New();
if (a == SurfaceType::Motor)
return RotateV::New();
return std::nullopt;
}
void Part::MakeJoints() {
// Algorithm: Find nearby parts
// Make sure parts are not dependant on each other (via primary/secondaryJoints)
// Find matching surfaces (surface normal dot product < -0.999)
// Get surface cframe of this part
// Transform surface center of other part to local via surface cframe of this part
// Make sure z of transformed center is not greater than 0.05
if (!workspace()) return;
// TEMPORARY
// TODO: Use more efficient algorithm to *actually* find nearby parts)
for (auto it = workspace().value()->GetDescendantsStart(); it != workspace().value()->GetDescendantsEnd(); it++) {
std::shared_ptr<Instance> obj = *it;
if (obj == shared_from_this()) continue; // Skip ourselves
if (obj->GetClass()->className != "Part") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
std::shared_ptr<Part> otherPart = obj->CastTo<Part>().expect();
for (Vector3 myFace : FACES) {
Vector3 myWorldNormal = cframe.Rotation() * myFace;
Vector3 validUp = cframe.Rotation() * Vector3(1,1,1).Unit(); // If myFace == (0, 1, 0), then (0, 1, 0) would produce NaN as up, so we fudge the up so that it works
CFrame surfaceFrame = CFrame::pointToward(cframe * (myFace * size / 2.f), cframe.Rotation() * myFace);
Vector3 mySurfaceCenter = cframe * (myFace * size / 2.f);
for (Vector3 otherFace : FACES) {
Vector3 otherWorldNormal = otherPart->cframe.Rotation() * otherFace;
Vector3 otherSurfaceCenter = otherPart->cframe * (otherFace * otherPart->size / 2.f);
Vector3 surfacePointLocalToMyFrame = surfaceFrame.Inverse() * otherSurfaceCenter;
float dot = myWorldNormal.Dot(otherWorldNormal);
if (dot > -0.99) continue; // Surface is pointing opposite to ours
if (abs(surfacePointLocalToMyFrame.Z()) > 0.05) continue; // Surfaces are within 0.05 studs of one another
if (!checkSurfacesTouching(surfaceFrame, size, myFace, otherFace, otherPart)) continue; // Surface do not overlap
if (!checkJointContinuity(otherPart)) continue;
SurfaceType mySurface = surfaceFromFace(faceFromNormal(myFace));
SurfaceType otherSurface = surfaceFromFace(faceFromNormal(otherFace));
// If it is a hinge, only attach if actually touching the "hinge"
if ((mySurface == SurfaceType::Hinge || mySurface == SurfaceType::Motor) && !checkSurfacesTouching(surfaceFrame, Vector3(0.4, 0.4, 0.4), myFace, otherFace, otherPart)) continue;
// Create contacts
// Contact always occurs at the center of Part0's surface (even if that point does not overlap both surfaces)
// Contact 0 is Part0's contact relative to Part0. It should point *opposite* the direction of its surface normal
// Contact 1 is Part1's contact relative to Part1. It should point directly toward the direction of its surface normal
// My additional notes:
// Contact == Part0.CFrame * C0 == Part1.CFrame * C1
// C1 == Part1.CFrame:Inverse() * Part0.CFrame * C0
// Part1.CFrame == Part0.CFrame * C0 * C1:Inverse()
// C0 == Part0.CFrame:Inverse() * Contact
CFrame contactPoint = CFrame::pointToward(mySurfaceCenter, -myWorldNormal);
CFrame contact0 = cframe.Inverse() * contactPoint;
CFrame contact1 = otherPart->cframe.Inverse() * contactPoint;
auto joint_ = makeJointFromSurfaces(mySurface, otherSurface);
if (!joint_) continue;
std::shared_ptr<JointInstance> joint = joint_.value();
joint->part0 = shared<Part>();
joint->part1 = otherPart->shared<Part>();
joint->c0 = contact0;
joint->c1 = contact1;
dataModel().value()->GetService<JointsService>()->AddChild(joint);
joint->UpdateProperty("Part0");
Logger::debugf("Made joint between %s and %s!\n", name.c_str(), otherPart->name.c_str());
}
}
}
}
void Part::trackJoint(std::shared_ptr<JointInstance> joint) {
if (!joint->part0.expired() && joint->part0.lock() == shared_from_this()) {
for (auto it = primaryJoints.begin(); it != primaryJoints.end();) {
// Clean expired refs
if (it->expired()) {
primaryJoints.erase(it);
continue;
}
// If the joint is already tracked, skip
if (it->lock() == joint)
return;
it++;
}
primaryJoints.push_back(joint);
} else if (!joint->part1.expired() && joint->part1.lock() == shared_from_this()) {
for (auto it = secondaryJoints.begin(); it != secondaryJoints.end();) {
// Clean expired refs
if (it->expired()) {
secondaryJoints.erase(it);
continue;
}
// If the joint is already tracked, skip
if (it->lock() == joint)
return;
it++;
}
secondaryJoints.push_back(joint);
}
}
void Part::untrackJoint(std::shared_ptr<JointInstance> joint) {
for (auto it = primaryJoints.begin(); it != primaryJoints.end();) {
// Clean expired refs
if (it->expired() || it->lock() == joint) {
primaryJoints.erase(it);
continue;
}
it++;
}
for (auto it = secondaryJoints.begin(); it != secondaryJoints.end();) {
// Clean expired refs
if (it->expired() || it->lock() == joint) {
secondaryJoints.erase(it);
continue;
}
it++;
}
}
Reference in a new issue View git blame Copy permalink