okooooooo/EVEngine
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
45e4aedabb3fa5fb854dd26dec0c2ba17da79f92
EVEngine/src/modules/physics/Body3D.cpp
T

848 lines
34 KiB
C++
Raw Blame History

#include "physics/Body3D.h"
#include "physics/Shape3D.h"
#include "physics/World3D.h"
#include "physics/Joint3D.h"
#include "common/Exception.h"
#include <box3d/box3d.h>
#include <cmath>
#include <vector>
namespace eve::physics {
namespace {
template <class T>
eve::Result<T> bodyValueFailure(eve::DiagnosticCode code, std::string message, std::string path) {
return eve::Result<T>::failure(
eve::Diagnostic::error(code, std::move(message), std::move(path), {}, "physics.body3d"));
}
eve::Result<void> validateOwnedTransformInput(const Body3D& body, float x, float y, float z) {
if (!body.isValid())
return bodyValueFailure<void>(eve::DiagnosticCode::StaleHandle,
"physics body is no longer valid", "body");
if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z))
return bodyValueFailure<void>(eve::DiagnosticCode::InvalidArgument,
"transform input must be finite", "value");
return eve::Result<void>::success();
}
b3BodyType parseBodyType(const std::string &type) {
if (type == "static") return b3_staticBody;
if (type == "kinematic") return b3_kinematicBody;
if (type == "dynamic") return b3_dynamicBody;
throw eve::Exception("Body3D.setType: unknown body type '%s'", type.c_str());
}
const char *bodyTypeName(b3BodyType t) {
switch (t) {
case b3_staticBody: return "static";
case b3_kinematicBody: return "kinematic";
case b3_dynamicBody: return "dynamic";
default: return "static";
}
}
void requireFinite(float value, const char *operation, const char *parameter) {
if (!std::isfinite(value))
throw eve::Exception("%s: %s must be finite", operation, parameter);
}
void requireNonNegative(float value, const char *operation, const char *parameter) {
requireFinite(value, operation, parameter);
if (value < 0.f)
throw eve::Exception("%s: %s must be >= 0", operation, parameter);
}
b3Vec3 checkedVector(float x, float y, float z, const char *operation,
const char *parameter) {
if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z))
throw eve::Exception("%s: %s components must be finite", operation, parameter);
return {x, y, z};
}
b3Quat normalizedQuaternion(float qx, float qy, float qz, float qw, const char *operation) {
requireFinite(qx, operation, "qx");
requireFinite(qy, operation, "qy");
requireFinite(qz, operation, "qz");
requireFinite(qw, operation, "qw");
const float lengthSquared = qx * qx + qy * qy + qz * qz + qw * qw;
if (lengthSquared <= 1e-16f)
throw eve::Exception("%s: quaternion length must be > 0", operation);
const float inverseLength = 1.f / std::sqrt(lengthSquared);
return b3Quat{{qx * inverseLength, qy * inverseLength, qz * inverseLength},
qw * inverseLength};
}
b3ShapeDef makeShapeDef(float density, float friction, float restitution) {
b3ShapeDef def = b3DefaultShapeDef();
def.density = density;
def.baseMaterial.friction = friction;
def.baseMaterial.restitution = restitution;
def.enableContactEvents = true;
def.enableSensorEvents = true;
return def;
}
b3HullData *createCheckedHull(const std::vector<float> &vertices, int maxVertices,
const char *operation) {
if (vertices.size() < 12 || vertices.size() % 3 != 0)
throw eve::Exception("%s: vertices must contain at least four packed XYZ points",
operation);
if (vertices.size() / 3 > 100000)
throw eve::Exception("%s: source point count must be <= 100000", operation);
if (maxVertices < 4 || maxVertices > 254)
throw eve::Exception("%s: maxVertices must be in [4, 254]", operation);
std::vector<b3Vec3> points;
points.reserve(vertices.size() / 3);
for (size_t i = 0; i < vertices.size(); i += 3) {
if (!std::isfinite(vertices[i]) || !std::isfinite(vertices[i + 1]) ||
!std::isfinite(vertices[i + 2]))
throw eve::Exception("%s: all vertex components must be finite", operation);
points.push_back({vertices[i], vertices[i + 1], vertices[i + 2]});
}
b3HullData *hull = b3CreateHull(points.data(), static_cast<int>(points.size()), maxVertices);
if (!hull)
throw eve::Exception("%s: points do not form a valid three-dimensional convex hull",
operation);
return hull;
}
b3MeshData *createCheckedMesh(const std::vector<float> &vertices,
const std::vector<int32_t> &indices, bool weldVertices,
float weldTolerance, bool identifyEdges, bool useMedianSplit,
const char *operation) {
if (vertices.size() < 9 || vertices.size() % 3 != 0)
throw eve::Exception("%s: vertices must contain at least three packed XYZ points",
operation);
if (indices.size() < 3 || indices.size() % 3 != 0)
throw eve::Exception("%s: indices must contain complete triangles", operation);
if (vertices.size() / 3 > 1000000 || indices.size() / 3 > 2000000)
throw eve::Exception("%s: mesh exceeds 1000000 vertices or 2000000 triangles",
operation);
if (!std::isfinite(weldTolerance) || weldTolerance < 0.f)
throw eve::Exception("%s: weldTolerance must be finite and >= 0", operation);
std::vector<b3Vec3> points;
points.reserve(vertices.size() / 3);
for (size_t i = 0; i < vertices.size(); i += 3) {
if (!std::isfinite(vertices[i]) || !std::isfinite(vertices[i + 1]) ||
!std::isfinite(vertices[i + 2]))
throw eve::Exception("%s: all vertex components must be finite", operation);
points.push_back({vertices[i], vertices[i + 1], vertices[i + 2]});
}
for (int32_t index : indices) {
if (index < 0 || static_cast<size_t>(index) >= points.size())
throw eve::Exception("%s: triangle index is outside the vertex array", operation);
}
std::vector<int32_t> mutableIndices = indices;
b3MeshDef def{};
def.vertices = points.data();
def.indices = mutableIndices.data();
def.vertexCount = static_cast<int>(points.size());
def.triangleCount = static_cast<int>(indices.size() / 3);
def.weldVertices = weldVertices;
def.weldTolerance = weldTolerance;
def.identifyEdges = identifyEdges;
def.useMedianSplit = useMedianSplit;
b3MeshData *mesh = b3CreateMesh(&def, nullptr, 0);
if (!mesh || mesh->triangleCount != def.triangleCount) {
if (mesh) b3DestroyMesh(mesh);
throw eve::Exception("%s: mesh contains degenerate or zero-area triangles", operation);
}
return mesh;
}
b3HeightFieldData *createCheckedHeightField(int countX, int countZ, float cellSizeX,
float cellSizeZ,
const std::vector<float> &heights,
float globalMin, float globalMax,
bool clockwiseWinding, const char *operation) {
if (countX < 2 || countZ < 2)
throw eve::Exception("%s: countX and countZ must be >= 2", operation);
const size_t sampleCount = static_cast<size_t>(countX) * static_cast<size_t>(countZ);
if (sampleCount > 16000000)
throw eve::Exception("%s: sample count must be <= 16000000", operation);
if (heights.size() != sampleCount)
throw eve::Exception("%s: heights size must equal countX * countZ", operation);
if (!(cellSizeX > 0.f) || !(cellSizeZ > 0.f) || !std::isfinite(cellSizeX) ||
!std::isfinite(cellSizeZ))
throw eve::Exception("%s: cell sizes must be finite and > 0", operation);
if (!std::isfinite(globalMin) || !std::isfinite(globalMax) || globalMin > globalMax)
throw eve::Exception("%s: global height range must be finite and ordered", operation);
for (float height : heights) {
if (!std::isfinite(height) || height < globalMin || height > globalMax)
throw eve::Exception("%s: every height must be finite and inside global range",
operation);
}
std::vector<float> mutableHeights = heights;
b3HeightFieldDef def{};
def.heights = mutableHeights.data();
def.scale = {cellSizeX, 1.f, cellSizeZ};
def.countX = countX;
def.countZ = countZ;
def.globalMinimumHeight = globalMin;
def.globalMaximumHeight = globalMax;
def.clockwiseWinding = clockwiseWinding;
return b3CreateHeightField(&def);
}
} // namespace
Body3D::Body3D(World3D *world, b3BodyId bodyId, int id, PhysicsBodyHandle runtimeHandle)
: world_(world), bodyId_(bodyId), id_(id), runtimeHandle_(runtimeHandle) {}
Body3D::~Body3D() {
if (isValid() && world_ && world_->isValid()) {
std::vector<Joint3D *> joints(world_->joints_.begin(), world_->joints_.end());
for (Joint3D *joint : joints) {
if (joint && (joint->bodyA_ == this || joint->bodyB_ == this)) {
world_->forgetJoint(joint);
joint->invalidate();
}
}
// Invalidate shapes that still reference this body.
std::vector<Shape3D *> shapes(world_->shapes_.begin(), world_->shapes_.end());
for (Shape3D *s : shapes) {
if (s && s->getBody() == this) {
world_->forgetShape(s);
s->invalidate();
}
}
b3Body_SetUserData(bodyId_, nullptr);
b3DestroyBody(bodyId_);
world_->forgetBody(this);
}
bodyId_ = {};
world_ = nullptr;
runtimeHandle_ = PhysicsBodyHandle::invalid();
}
bool Body3D::isValid() const { return b3Body_IsValid(bodyId_); }
void Body3D::invalidate() {
if (isValid()) b3Body_SetUserData(bodyId_, nullptr);
bodyId_ = {};
world_ = nullptr;
runtimeHandle_ = PhysicsBodyHandle::invalid();
}
void Body3D::destroy() {
if (!isValid() || !world_ || !world_->isValid()) {
invalidate();
return;
}
std::vector<Joint3D *> joints(world_->joints_.begin(), world_->joints_.end());
for (Joint3D *joint : joints) {
if (joint && (joint->bodyA_ == this || joint->bodyB_ == this)) {
world_->forgetJoint(joint);
joint->invalidate();
}
}
std::vector<Shape3D *> shapes(world_->shapes_.begin(), world_->shapes_.end());
for (Shape3D *s : shapes) {
if (s && s->getBody() == this) {
world_->forgetShape(s);
// Destroy the underlying shape before invalidating the wrapper.
if (s->isValid()) b3DestroyShape(s->raw(), false);
s->invalidate();
}
}
b3Body_SetUserData(bodyId_, nullptr);
b3DestroyBody(bodyId_);
world_->forgetBody(this);
bodyId_ = {};
world_ = nullptr;
runtimeHandle_ = PhysicsBodyHandle::invalid();
}
void Body3D::setPosition(float x, float y, float z) {
if (!isValid()) return;
b3Body_SetTransform(bodyId_, b3Pos{x, y, z}, b3Body_GetRotation(bodyId_));
}
float Body3D::getX() const {
if (!isValid()) return 0.f;
return static_cast<float>(b3Body_GetPosition(bodyId_).x);
}
float Body3D::getY() const {
if (!isValid()) return 0.f;
return static_cast<float>(b3Body_GetPosition(bodyId_).y);
}
float Body3D::getZ() const {
if (!isValid()) return 0.f;
return static_cast<float>(b3Body_GetPosition(bodyId_).z);
}
void Body3D::setRotation(float qx, float qy, float qz, float qw) {
if (!isValid()) return;
b3Quat q;
q.v = b3Vec3{qx, qy, qz};
q.s = qw;
float len = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
if (len > 1e-8f) {
q.v.x /= len;
q.v.y /= len;
q.v.z /= len;
q.s /= len;
} else {
q = b3Quat_identity;
}
b3Body_SetTransform(bodyId_, b3Body_GetPosition(bodyId_), q);
}
float Body3D::getRotX() const {
if (!isValid()) return 0.f;
return b3Body_GetRotation(bodyId_).v.x;
}
float Body3D::getRotY() const {
if (!isValid()) return 0.f;
return b3Body_GetRotation(bodyId_).v.y;
}
float Body3D::getRotZ() const {
if (!isValid()) return 0.f;
return b3Body_GetRotation(bodyId_).v.z;
}
float Body3D::getRotW() const {
if (!isValid()) return 1.f;
return b3Body_GetRotation(bodyId_).s;
}
void Body3D::setLinearVelocity(float vx, float vy, float vz) {
if (!isValid()) return;
b3Body_SetLinearVelocity(bodyId_, b3Vec3{vx, vy, vz});
}
float Body3D::getLinearVelocityX() const {
if (!isValid()) return 0.f;
return b3Body_GetLinearVelocity(bodyId_).x;
}
float Body3D::getLinearVelocityY() const {
if (!isValid()) return 0.f;
return b3Body_GetLinearVelocity(bodyId_).y;
}
float Body3D::getLinearVelocityZ() const {
if (!isValid()) return 0.f;
return b3Body_GetLinearVelocity(bodyId_).z;
}
float Body3D::getMass() const {
if (!isValid()) return 0.f;
return b3Body_GetMass(bodyId_);
}
void Body3D::setAngularVelocity(float wx, float wy, float wz) {
if (!isValid()) return;
b3Body_SetAngularVelocity(bodyId_, b3Vec3{wx, wy, wz});
}
float Body3D::getAngularVelocityX() const {
if (!isValid()) return 0.f;
return b3Body_GetAngularVelocity(bodyId_).x;
}
float Body3D::getAngularVelocityY() const {
if (!isValid()) return 0.f;
return b3Body_GetAngularVelocity(bodyId_).y;
}
float Body3D::getAngularVelocityZ() const {
if (!isValid()) return 0.f;
return b3Body_GetAngularVelocity(bodyId_).z;
}
std::vector<float> Body3D::localToWorldPoint(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Pos value = b3Body_GetWorldPoint(
bodyId_, checkedVector(x, y, z, "Body3D.localToWorldPoint", "point"));
return {static_cast<float>(value.x), static_cast<float>(value.y),
static_cast<float>(value.z)};
}
eve::Result<PhysicsVector3D> Body3D::localToWorldPointOwned(float x, float y, float z) const {
auto valid = validateOwnedTransformInput(*this, x, y, z);
if (!valid) return eve::Result<PhysicsVector3D>::failure(valid.status());
const b3Pos value = b3Body_GetWorldPoint(bodyId_, b3Vec3{x, y, z});
return eve::Result<PhysicsVector3D>::success(
{static_cast<float>(value.x), static_cast<float>(value.y), static_cast<float>(value.z)});
}
std::vector<float> Body3D::worldToLocalPoint(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Vec3 value = b3Body_GetLocalPoint(
bodyId_, checkedVector(x, y, z, "Body3D.worldToLocalPoint", "point"));
return {value.x, value.y, value.z};
}
eve::Result<PhysicsVector3D> Body3D::worldToLocalPointOwned(float x, float y, float z) const {
auto valid = validateOwnedTransformInput(*this, x, y, z);
if (!valid) return eve::Result<PhysicsVector3D>::failure(valid.status());
const b3Vec3 value = b3Body_GetLocalPoint(bodyId_, b3Vec3{x, y, z});
return eve::Result<PhysicsVector3D>::success({value.x, value.y, value.z});
}
std::vector<float> Body3D::localToWorldVector(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Vec3 value = b3Body_GetWorldVector(
bodyId_, checkedVector(x, y, z, "Body3D.localToWorldVector", "vector"));
return {value.x, value.y, value.z};
}
eve::Result<PhysicsVector3D> Body3D::localToWorldVectorOwned(float x, float y, float z) const {
auto valid = validateOwnedTransformInput(*this, x, y, z);
if (!valid) return eve::Result<PhysicsVector3D>::failure(valid.status());
const b3Vec3 value = b3Body_GetWorldVector(bodyId_, b3Vec3{x, y, z});
return eve::Result<PhysicsVector3D>::success({value.x, value.y, value.z});
}
std::vector<float> Body3D::worldToLocalVector(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Vec3 value = b3Body_GetLocalVector(
bodyId_, checkedVector(x, y, z, "Body3D.worldToLocalVector", "vector"));
return {value.x, value.y, value.z};
}
std::vector<float> Body3D::getLocalPointVelocity(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Vec3 value = b3Body_GetLocalPointVelocity(
bodyId_, checkedVector(x, y, z, "Body3D.getLocalPointVelocity", "point"));
return {value.x, value.y, value.z};
}
eve::Result<PhysicsVector3D> Body3D::getLocalPointVelocityOwned(float x, float y, float z) const {
auto valid = validateOwnedTransformInput(*this, x, y, z);
if (!valid) return eve::Result<PhysicsVector3D>::failure(valid.status());
const b3Vec3 value = b3Body_GetLocalPointVelocity(bodyId_, b3Vec3{x, y, z});
return eve::Result<PhysicsVector3D>::success({value.x, value.y, value.z});
}
std::vector<float> Body3D::getWorldPointVelocity(float x, float y, float z) const {
if (!isValid()) return {0.f, 0.f, 0.f};
const b3Vec3 value = b3Body_GetWorldPointVelocity(
bodyId_, checkedVector(x, y, z, "Body3D.getWorldPointVelocity", "point"));
return {value.x, value.y, value.z};
}
void Body3D::applyForce(float fx, float fy, float fz) {
if (!isValid()) return;
b3Body_ApplyForceToCenter(bodyId_, b3Vec3{fx, fy, fz}, true);
}
void Body3D::applyForceAt(float fx, float fy, float fz, float x, float y, float z) {
if (!isValid()) return;
b3Body_ApplyForce(bodyId_, b3Vec3{fx, fy, fz}, b3Pos{x, y, z}, true);
}
void Body3D::applyTorque(float tx, float ty, float tz) {
if (!isValid()) return;
b3Body_ApplyTorque(bodyId_, b3Vec3{tx, ty, tz}, true);
}
void Body3D::applyLinearImpulse(float ix, float iy, float iz) {
if (!isValid()) return;
b3Body_ApplyLinearImpulseToCenter(bodyId_, b3Vec3{ix, iy, iz}, true);
}
void Body3D::applyLinearImpulseAt(float ix, float iy, float iz, float x, float y, float z) {
if (!isValid()) return;
b3Body_ApplyLinearImpulse(bodyId_, b3Vec3{ix, iy, iz}, b3Pos{x, y, z}, true);
}
void Body3D::applyAngularImpulse(float ix, float iy, float iz) {
if (!isValid()) return;
b3Body_ApplyAngularImpulse(bodyId_, b3Vec3{ix, iy, iz}, true);
}
void Body3D::applyLocalForce(float fx, float fy, float fz, float x, float y, float z) {
if (!isValid()) return;
const b3Vec3 force = b3Body_GetWorldVector(
bodyId_, checkedVector(fx, fy, fz, "Body3D.applyLocalForce", "force"));
const b3Pos point = b3Body_GetWorldPoint(
bodyId_, checkedVector(x, y, z, "Body3D.applyLocalForce", "point"));
b3Body_ApplyForce(bodyId_, force, point, true);
}
void Body3D::applyLocalForceToCenter(float fx, float fy, float fz) {
if (!isValid()) return;
const b3Vec3 force = checkedVector(fx, fy, fz, "Body3D.applyLocalForceToCenter",
"force");
b3Body_ApplyForceToCenter(bodyId_, b3Body_GetWorldVector(bodyId_, force), true);
}
void Body3D::applyLocalTorque(float tx, float ty, float tz) {
if (!isValid()) return;
const b3Vec3 torque = checkedVector(tx, ty, tz, "Body3D.applyLocalTorque", "torque");
b3Body_ApplyTorque(bodyId_, b3Body_GetWorldVector(bodyId_, torque), true);
}
void Body3D::applyLocalLinearImpulse(float ix, float iy, float iz, float x, float y,
float z) {
if (!isValid()) return;
const b3Vec3 impulse = b3Body_GetWorldVector(
bodyId_, checkedVector(ix, iy, iz, "Body3D.applyLocalLinearImpulse", "impulse"));
const b3Pos point = b3Body_GetWorldPoint(
bodyId_, checkedVector(x, y, z, "Body3D.applyLocalLinearImpulse", "point"));
b3Body_ApplyLinearImpulse(bodyId_, impulse, point, true);
}
void Body3D::applyLocalLinearImpulseToCenter(float ix, float iy, float iz) {
if (!isValid()) return;
const b3Vec3 impulse = checkedVector(ix, iy, iz,
"Body3D.applyLocalLinearImpulseToCenter", "impulse");
b3Body_ApplyLinearImpulseToCenter(bodyId_, b3Body_GetWorldVector(bodyId_, impulse), true);
}
void Body3D::applyLocalAngularImpulse(float ix, float iy, float iz) {
if (!isValid()) return;
const b3Vec3 impulse =
checkedVector(ix, iy, iz, "Body3D.applyLocalAngularImpulse", "impulse");
b3Body_ApplyAngularImpulse(bodyId_, b3Body_GetWorldVector(bodyId_, impulse), true);
}
void Body3D::setTargetTransform(float x, float y, float z, float qx, float qy, float qz,
float qw, float timeStep) {
if (!isValid()) return;
requireFinite(x, "Body3D.setTargetTransform", "x");
requireFinite(y, "Body3D.setTargetTransform", "y");
requireFinite(z, "Body3D.setTargetTransform", "z");
requireFinite(timeStep, "Body3D.setTargetTransform", "timeStep");
if (timeStep <= 0.f)
throw eve::Exception("Body3D.setTargetTransform: timeStep must be > 0");
const b3Quat rotation =
normalizedQuaternion(qx, qy, qz, qw, "Body3D.setTargetTransform");
b3Body_SetTargetTransform(bodyId_, b3WorldTransform{b3Pos{x, y, z}, rotation}, timeStep,
true);
}
void Body3D::setLinearDamping(float damping) {
if (!isValid()) return;
requireNonNegative(damping, "Body3D.setLinearDamping", "damping");
b3Body_SetLinearDamping(bodyId_, damping);
}
float Body3D::getLinearDamping() const {
return isValid() ? b3Body_GetLinearDamping(bodyId_) : 0.f;
}
void Body3D::setAngularDamping(float damping) {
if (!isValid()) return;
requireNonNegative(damping, "Body3D.setAngularDamping", "damping");
b3Body_SetAngularDamping(bodyId_, damping);
}
float Body3D::getAngularDamping() const {
return isValid() ? b3Body_GetAngularDamping(bodyId_) : 0.f;
}
void Body3D::setGravityScale(float scale) {
if (!isValid()) return;
requireFinite(scale, "Body3D.setGravityScale", "scale");
b3Body_SetGravityScale(bodyId_, scale);
}
float Body3D::getGravityScale() const {
return isValid() ? b3Body_GetGravityScale(bodyId_) : 0.f;
}
void Body3D::setSleepEnabled(bool enabled) {
if (!isValid()) return;
b3Body_EnableSleep(bodyId_, enabled);
}
bool Body3D::isSleepEnabled() const {
return isValid() ? b3Body_IsSleepEnabled(bodyId_) : false;
}
void Body3D::setSleepThreshold(float threshold) {
if (!isValid()) return;
requireNonNegative(threshold, "Body3D.setSleepThreshold", "threshold");
b3Body_SetSleepThreshold(bodyId_, threshold);
}
float Body3D::getSleepThreshold() const {
return isValid() ? b3Body_GetSleepThreshold(bodyId_) : 0.f;
}
void Body3D::setMotionLocks(bool linearX, bool linearY, bool linearZ, bool angularX,
bool angularY, bool angularZ) {
if (!isValid()) return;
b3Body_SetMotionLocks(bodyId_,
b3MotionLocks{linearX, linearY, linearZ, angularX, angularY,
angularZ});
}
#define EV_BODY_LOCK_GETTER(name, member) \
bool Body3D::name() const { \
return isValid() ? b3Body_GetMotionLocks(bodyId_).member : false; \
}
EV_BODY_LOCK_GETTER(isLinearXLocked, linearX)
EV_BODY_LOCK_GETTER(isLinearYLocked, linearY)
EV_BODY_LOCK_GETTER(isLinearZLocked, linearZ)
EV_BODY_LOCK_GETTER(isAngularXLocked, angularX)
EV_BODY_LOCK_GETTER(isAngularYLocked, angularY)
EV_BODY_LOCK_GETTER(isAngularZLocked, angularZ)
#undef EV_BODY_LOCK_GETTER
void Body3D::setMassProperties(float mass, float centerX, float centerY, float centerZ,
float inertiaXX, float inertiaYY, float inertiaZZ,
float inertiaXY, float inertiaXZ, float inertiaYZ) {
if (!isValid()) return;
constexpr const char *operation = "Body3D.setMassProperties";
if (b3Body_GetType(bodyId_) != b3_dynamicBody)
throw eve::Exception("%s: body must be dynamic", operation);
requireFinite(mass, operation, "mass");
requireFinite(centerX, operation, "centerX");
requireFinite(centerY, operation, "centerY");
requireFinite(centerZ, operation, "centerZ");
requireFinite(inertiaXX, operation, "inertiaXX");
requireFinite(inertiaYY, operation, "inertiaYY");
requireFinite(inertiaZZ, operation, "inertiaZZ");
requireFinite(inertiaXY, operation, "inertiaXY");
requireFinite(inertiaXZ, operation, "inertiaXZ");
requireFinite(inertiaYZ, operation, "inertiaYZ");
if (!(mass > 0.f)) throw eve::Exception("%s: mass must be > 0", operation);
// Sylvester's criterion for a symmetric positive-definite 3x3 tensor.
const double xx = inertiaXX, yy = inertiaYY, zz = inertiaZZ;
const double xy = inertiaXY, xz = inertiaXZ, yz = inertiaYZ;
const double minor2 = xx * yy - xy * xy;
const double determinant =
xx * (yy * zz - yz * yz) - xy * (xy * zz - yz * xz) +
xz * (xy * yz - yy * xz);
if (!(xx > 0.0 && minor2 > 0.0 && determinant > 0.0) ||
!std::isfinite(minor2) || !std::isfinite(determinant))
throw eve::Exception("%s: inertia tensor must be positive definite", operation);
b3MassData data{};
data.mass = mass;
data.center = {centerX, centerY, centerZ};
data.inertia = {{inertiaXX, inertiaXY, inertiaXZ},
{inertiaXY, inertiaYY, inertiaYZ},
{inertiaXZ, inertiaYZ, inertiaZZ}};
b3Body_SetMassData(bodyId_, data);
}
void Body3D::resetMassProperties() {
if (!isValid()) return;
b3Body_ApplyMassFromShapes(bodyId_);
}
#define EV_BODY_INERTIA_GETTER(name, column, member) \
float Body3D::name() const { \
return isValid() ? b3Body_GetMassData(bodyId_).inertia.column.member : 0.f; \
}
EV_BODY_INERTIA_GETTER(getInertiaXX, cx, x)
EV_BODY_INERTIA_GETTER(getInertiaYY, cy, y)
EV_BODY_INERTIA_GETTER(getInertiaZZ, cz, z)
EV_BODY_INERTIA_GETTER(getInertiaXY, cx, y)
EV_BODY_INERTIA_GETTER(getInertiaXZ, cx, z)
EV_BODY_INERTIA_GETTER(getInertiaYZ, cy, z)
#undef EV_BODY_INERTIA_GETTER
#define EV_BODY_CENTER_GETTER(name, functionName, member) \
float Body3D::name() const { \
return isValid() ? static_cast<float>(functionName(bodyId_).member) : 0.f; \
}
EV_BODY_CENTER_GETTER(getLocalCenterX, b3Body_GetLocalCenterOfMass, x)
EV_BODY_CENTER_GETTER(getLocalCenterY, b3Body_GetLocalCenterOfMass, y)
EV_BODY_CENTER_GETTER(getLocalCenterZ, b3Body_GetLocalCenterOfMass, z)
EV_BODY_CENTER_GETTER(getWorldCenterX, b3Body_GetWorldCenterOfMass, x)
EV_BODY_CENTER_GETTER(getWorldCenterY, b3Body_GetWorldCenterOfMass, y)
EV_BODY_CENTER_GETTER(getWorldCenterZ, b3Body_GetWorldCenterOfMass, z)
#undef EV_BODY_CENTER_GETTER
void Body3D::setType(const std::string &bodyType) {
if (!isValid()) return;
const b3BodyType type = parseBodyType(bodyType);
if (type != b3_staticBody && world_) {
for (Shape3D *shape : world_->shapes_) {
if (shape && shape->getBody() == this &&
(shape->kind_ == Shape3D::Kind::TriangleMesh ||
shape->kind_ == Shape3D::Kind::HeightField))
throw eve::Exception(
"Body3D.setType: triangle-mesh and height-field colliders require a static body");
}
}
b3Body_SetType(bodyId_, type);
}
std::string Body3D::getType() const {
if (!isValid()) return "static";
return bodyTypeName(b3Body_GetType(bodyId_));
}
void Body3D::setFixedRotation(bool fixed) {
if (!isValid()) return;
b3MotionLocks locks = b3Body_GetMotionLocks(bodyId_);
locks.angularX = fixed;
locks.angularY = fixed;
locks.angularZ = fixed;
b3Body_SetMotionLocks(bodyId_, locks);
}
bool Body3D::isFixedRotation() const {
if (!isValid()) return false;
b3MotionLocks locks = b3Body_GetMotionLocks(bodyId_);
return locks.angularX && locks.angularY && locks.angularZ;
}
void Body3D::setActive(bool active) {
if (!isValid()) return;
if (active)
b3Body_Enable(bodyId_);
else
b3Body_Disable(bodyId_);
}
bool Body3D::isActive() const { return isValid() ? b3Body_IsEnabled(bodyId_) : false; }
void Body3D::setBullet(bool bullet) {
if (!isValid()) return;
b3Body_SetBullet(bodyId_, bullet);
}
bool Body3D::isBullet() const { return isValid() ? b3Body_IsBullet(bodyId_) : false; }
void Body3D::setAwake(bool awake) {
if (!isValid()) return;
b3Body_SetAwake(bodyId_, awake);
}
bool Body3D::isAwake() const { return isValid() ? b3Body_IsAwake(bodyId_) : false; }
Shape3D *Body3D::newBoxShape(float width, float height, float depth, float density,
float friction, float restitution) {
if (!isValid() || !world_) throw eve::Exception("Body3D.newBoxShape: body destroyed");
if (width <= 0.f || height <= 0.f || depth <= 0.f)
throw eve::Exception("Body3D.newBoxShape: width/height/depth must be > 0");
float hx = width * 0.5f;
float hy = height * 0.5f;
float hz = depth * 0.5f;
b3BoxHull box = b3MakeBoxHull(hx, hy, hz);
b3ShapeDef def = makeShapeDef(density, friction, restitution);
b3ShapeId id = b3CreateHullShape(bodyId_, &def, &box.base);
auto *shape = new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::Box, hx, hy, hz);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
Shape3D *Body3D::newSphereShape(float radius, float density, float friction, float restitution) {
if (!isValid() || !world_) throw eve::Exception("Body3D.newSphereShape: body destroyed");
if (radius <= 0.f) throw eve::Exception("Body3D.newSphereShape: radius must be > 0");
b3Sphere sphere;
sphere.center = b3Vec3_zero;
sphere.radius = radius;
b3ShapeDef def = makeShapeDef(density, friction, restitution);
b3ShapeId id = b3CreateSphereShape(bodyId_, &def, &sphere);
auto *shape =
new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::Sphere, radius, 0.f, 0.f);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
Shape3D *Body3D::newCapsuleShape(float height, float radius, float density, float friction,
float restitution) {
if (!isValid() || !world_) throw eve::Exception("Body3D.newCapsuleShape: body destroyed");
if (height < 0.f || radius <= 0.f)
throw eve::Exception("Body3D.newCapsuleShape: height >= 0 and radius > 0 required");
float half = height * 0.5f;
b3Capsule capsule;
capsule.center1 = b3Vec3{0.f, -half, 0.f};
capsule.center2 = b3Vec3{0.f, half, 0.f};
capsule.radius = radius;
b3ShapeDef def = makeShapeDef(density, friction, restitution);
b3ShapeId id = b3CreateCapsuleShape(bodyId_, &def, &capsule);
auto *shape =
new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::Capsule, half, radius, 0.f);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
Shape3D *Body3D::newConvexHullShape(const std::vector<float> &vertices, int maxVertices,
float density, float friction, float restitution) {
if (!isValid() || !world_)
throw eve::Exception("Body3D.newConvexHullShape: body destroyed");
b3HullData *hull = createCheckedHull(vertices, maxVertices, "Body3D.newConvexHullShape");
b3ShapeDef def = makeShapeDef(density, friction, restitution);
b3ShapeId id = b3CreateHullShape(bodyId_, &def, hull);
b3DestroyHull(hull);
auto *shape = new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::ConvexHull, 0.f, 0.f,
0.f, vertices, maxVertices);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
Shape3D *Body3D::newTriangleMeshShape(const std::vector<float> &vertices,
const std::vector<int32_t> &indices, bool weldVertices,
float weldTolerance, bool identifyEdges,
bool useMedianSplit) {
if (!isValid() || !world_)
throw eve::Exception("Body3D.newTriangleMeshShape: body destroyed");
if (b3Body_GetType(bodyId_) != b3_staticBody)
throw eve::Exception("Body3D.newTriangleMeshShape: triangle meshes require a static body");
b3MeshData *mesh = createCheckedMesh(vertices, indices, weldVertices, weldTolerance,
identifyEdges, useMedianSplit,
"Body3D.newTriangleMeshShape");
b3ShapeDef def = makeShapeDef(0.f, 0.2f, 0.f);
b3ShapeId id = b3CreateMeshShape(bodyId_, &def, mesh, b3Vec3_one);
if (B3_IS_NULL(id)) {
b3DestroyMesh(mesh);
throw eve::Exception("Body3D.newTriangleMeshShape: Box3D rejected the mesh shape");
}
auto *shape =
new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::TriangleMesh, 0.f, 0.f, 0.f, {},
64, vertices, indices, mesh, weldVertices, weldTolerance, identifyEdges, useMedianSplit);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
Shape3D *Body3D::newHeightFieldShape(int countX, int countZ, float cellSizeX,
float cellSizeZ, const std::vector<float> &heights,
float globalMin, float globalMax,
bool clockwiseWinding) {
if (!isValid() || !world_)
throw eve::Exception("Body3D.newHeightFieldShape: body destroyed");
if (b3Body_GetType(bodyId_) != b3_staticBody)
throw eve::Exception("Body3D.newHeightFieldShape: height fields require a static body");
b3HeightFieldData *heightData = createCheckedHeightField(
countX, countZ, cellSizeX, cellSizeZ, heights, globalMin, globalMax,
clockwiseWinding, "Body3D.newHeightFieldShape");
b3ShapeDef def = makeShapeDef(0.f, 0.2f, 0.f);
b3ShapeId id = b3CreateHeightFieldShape(bodyId_, &def, heightData);
if (B3_IS_NULL(id)) {
b3DestroyHeightField(heightData);
throw eve::Exception("Body3D.newHeightFieldShape: Box3D rejected the height field");
}
auto *shape = new Shape3D(world_, this, id, world_->nextShapeRuntimeHandle(), Shape3D::Kind::HeightField, 0.f, 0.f,
0.f, {}, 64, {}, {}, nullptr, true, 0.001f, true, false, heights, countX, countZ,
cellSizeX, cellSizeZ, globalMin, globalMax, clockwiseWinding, heightData);
b3Shape_SetUserData(id, shape);
world_->shapes_.insert(shape);
return shape;
}
} // namespace eve::physics
Reference in new issue View Git Blame Copy Permalink