initial commit, 4.5 stable
Some checks failed
🔗 GHA / 📊 Static checks (push) Has been cancelled
🔗 GHA / 🤖 Android (push) Has been cancelled
🔗 GHA / 🍏 iOS (push) Has been cancelled
🔗 GHA / 🐧 Linux (push) Has been cancelled
🔗 GHA / 🍎 macOS (push) Has been cancelled
🔗 GHA / 🏁 Windows (push) Has been cancelled
🔗 GHA / 🌐 Web (push) Has been cancelled

This commit is contained in:
2025-09-16 20:46:46 -04:00
commit 9d30169a8d
13378 changed files with 7050105 additions and 0 deletions

View File

@@ -0,0 +1,68 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has
enum class EAllowedDOFs : uint8
{
None = 0b000000, ///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead.
All = 0b111111, ///< All degrees of freedom are allowed
TranslationX = 0b000001, ///< Body can move in world space X axis
TranslationY = 0b000010, ///< Body can move in world space Y axis
TranslationZ = 0b000100, ///< Body can move in world space Z axis
RotationX = 0b001000, ///< Body can rotate around world space X axis
RotationY = 0b010000, ///< Body can rotate around world space Y axis
RotationZ = 0b100000, ///< Body can rotate around world space Z axis
Plane2D = TranslationX | TranslationY | RotationZ, ///< Body can only move in X and Y axis and rotate around Z axis
};
/// Bitwise OR operator for EAllowedDOFs
constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
{
return EAllowedDOFs(uint8(inLHS) | uint8(inRHS));
}
/// Bitwise AND operator for EAllowedDOFs
constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
{
return EAllowedDOFs(uint8(inLHS) & uint8(inRHS));
}
/// Bitwise XOR operator for EAllowedDOFs
constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
{
return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS));
}
/// Bitwise NOT operator for EAllowedDOFs
constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs)
{
return EAllowedDOFs(~uint8(inAllowedDOFs));
}
/// Bitwise OR assignment operator for EAllowedDOFs
constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
{
ioLHS = ioLHS | inRHS;
return ioLHS;
}
/// Bitwise AND assignment operator for EAllowedDOFs
constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
{
ioLHS = ioLHS & inRHS;
return ioLHS;
}
/// Bitwise XOR assignment operator for EAllowedDOFs
constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
{
ioLHS = ioLHS ^ inRHS;
return ioLHS;
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,423 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
#include <Jolt/Physics/PhysicsSettings.h>
#include <Jolt/Physics/StateRecorder.h>
#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
#include <Jolt/Core/StringTools.h>
#include <Jolt/Core/Profiler.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
static const EmptyShape sFixedToWorldShape;
Body Body::sFixedToWorld(false);
Body::Body(bool) :
mPosition(Vec3::sZero()),
mRotation(Quat::sIdentity()),
mShape(&sFixedToWorldShape), // Dummy shape
mFriction(0.0f),
mRestitution(0.0f),
mObjectLayer(cObjectLayerInvalid),
mMotionType(EMotionType::Static)
{
sFixedToWorldShape.SetEmbedded();
}
void Body::SetMotionType(EMotionType inMotionType)
{
if (mMotionType == inMotionType)
return;
JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set to true");
JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first");
JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0");
// Store new motion type
mMotionType = inMotionType;
if (mMotionProperties != nullptr)
{
// Update cache
JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;)
switch (inMotionType)
{
case EMotionType::Static:
// Stop the object
mMotionProperties->mLinearVelocity = Vec3::sZero();
mMotionProperties->mAngularVelocity = Vec3::sZero();
[[fallthrough]];
case EMotionType::Kinematic:
// Cancel forces
mMotionProperties->ResetForce();
mMotionProperties->ResetTorque();
break;
case EMotionType::Dynamic:
break;
}
}
}
void Body::SetAllowSleeping(bool inAllow)
{
mMotionProperties->mAllowSleeping = inAllow;
if (inAllow)
ResetSleepTimer();
}
void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
{
JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
JPH_ASSERT(!IsStatic());
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
// Calculate center of mass at end situation
RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
// Calculate delta position and rotation
Vec3 delta_pos = Vec3(new_com - mPosition);
Quat delta_rotation = inTargetRotation * mRotation.Conjugated();
mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime);
}
void Body::CalculateWorldSpaceBoundsInternal()
{
mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
}
void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer)
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
mPosition = inPosition + inRotation * mShape->GetCenterOfMass();
mRotation = inRotation;
// Initialize bounding box
CalculateWorldSpaceBoundsInternal();
// Reset sleeping test
if (inResetSleepTimer && mMotionProperties != nullptr)
ResetSleepTimer();
}
void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties)
{
// Update center of mass position so the world position for this body stays the same
mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass);
// Recalculate mass and inertia if requested
if (inUpdateMassProperties && mMotionProperties != nullptr)
mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties());
}
void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
{
JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
// Get the old center of mass
Vec3 old_com = mShape->GetCenterOfMass();
// Update the shape
mShape = inShape;
// Update center of mass
UpdateCenterOfMassInternal(old_com, inUpdateMassProperties);
// Recalculate bounding box
CalculateWorldSpaceBoundsInternal();
}
ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
{
// Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies)
if (!mMotionProperties->mAllowSleeping || IsSensor())
return ECanSleep::CannotSleep;
// Get the points to test
RVec3 points[3];
GetSleepTestPoints(points);
#ifdef JPH_DOUBLE_PRECISION
// Get base offset for spheres
DVec3 offset = mMotionProperties->GetSleepTestOffset();
#endif // JPH_DOUBLE_PRECISION
for (int i = 0; i < 3; ++i)
{
Sphere &sphere = mMotionProperties->mSleepTestSpheres[i];
// Make point relative to base offset
#ifdef JPH_DOUBLE_PRECISION
Vec3 p = Vec3(points[i] - offset);
#else
Vec3 p = points[i];
#endif // JPH_DOUBLE_PRECISION
// Encapsulate the point in a sphere
sphere.EncapsulatePoint(p);
// Test if it exceeded the max movement
if (sphere.GetRadius() > inMaxMovement)
{
// Body is not sleeping, reset test
mMotionProperties->ResetSleepTestSpheres(points);
return ECanSleep::CannotSleep;
}
}
return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep);
}
void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const
{
// For GetSubmergedVolume we transform the surface relative to the body position for increased precision
Mat44 rotation = Mat44::sRotation(mRotation);
Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal);
// Calculate amount of volume that is submerged and what the center of buoyancy is
mShape->GetSubmergedVolume(rotation, Vec3::sOne(), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
}
bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
{
JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently
// We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra
// All quantities below are in world space
// If we're not submerged, there's no point in doing the rest of the calculations
if (inSubmergedVolume > 0.0f)
{
#ifdef JPH_DEBUG_RENDERER
// Draw submerged volume properties
if (Shape::sDrawSubmergedVolumes)
{
RVec3 center_of_buoyancy = mPosition + inRelativeCenterOfBuoyancy;
DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f);
DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)inSubmergedVolume, (double)inTotalVolume));
}
#endif // JPH_DEBUG_RENDERER
// When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point.
// Buoyancy > 1 should make the object float, < 1 should make it sink.
float inverse_mass = mMotionProperties->GetInverseMass();
float fluid_density = inBuoyancy / (inTotalVolume * inverse_mass);
// Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1)
// Impulse = Force * Delta time
// We should apply this at the center of buoyancy (= center of mass of submerged volume)
Vec3 buoyancy_impulse = -fluid_density * inSubmergedVolume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime;
// Calculate the velocity of the center of buoyancy relative to the fluid
Vec3 linear_velocity = mMotionProperties->GetLinearVelocity();
Vec3 angular_velocity = mMotionProperties->GetAngularVelocity();
Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(inRelativeCenterOfBuoyancy);
Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity;
// Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29
// Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity
// Again Impulse = Force * Delta Time
// We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset)
// Get size of local bounding box
Vec3 size = mShape->GetLocalBounds().GetSize();
// Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy
float area = 0.0f;
float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq();
if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f)
{
Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity;
area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * size.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>()) / sqrt(relative_center_of_buoyancy_velocity_len_sq);
}
// Calculate the impulse
Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length();
// Clamp magnitude against current linear velocity to prevent overshoot
float linear_velocity_len_sq = linear_velocity.LengthSq();
float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq();
if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq)
drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq);
// Calculate the resulting delta linear velocity due to buoyancy and drag
Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass;
mMotionProperties->AddLinearVelocityStep(delta_linear_velocity);
// Determine average width of the body (across the three axis)
float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f;
// Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15)
Vec3 drag_angular_impulse = (-inAngularDrag * inSubmergedVolume / inTotalVolume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity;
Mat44 inv_inertia = GetInverseInertia();
Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse;
// Clamp magnitude against the current angular velocity to prevent overshoot
float angular_velocity_len_sq = angular_velocity.LengthSq();
float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq();
if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq)
drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq);
// Calculate total delta angular velocity due to drag and buoyancy
Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * inRelativeCenterOfBuoyancy.Cross(buoyancy_impulse + drag_impulse);
mMotionProperties->AddAngularVelocityStep(delta_angular_velocity);
return true;
}
return false;
}
bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
{
JPH_PROFILE_FUNCTION();
float total_volume, submerged_volume;
Vec3 relative_center_of_buoyancy;
GetSubmergedVolume(inSurfacePosition, inSurfaceNormal, total_volume, submerged_volume, relative_center_of_buoyancy);
return ApplyBuoyancyImpulse(total_volume, submerged_volume, relative_center_of_buoyancy, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime);
}
void Body::SaveState(StateRecorder &inStream) const
{
// Only write properties that can change at runtime
inStream.Write(mPosition);
inStream.Write(mRotation);
if (mMotionProperties != nullptr)
{
if (IsSoftBody())
static_cast<const SoftBodyMotionProperties *>(mMotionProperties)->SaveState(inStream);
else
mMotionProperties->SaveState(inStream);
}
}
void Body::RestoreState(StateRecorder &inStream)
{
inStream.Read(mPosition);
inStream.Read(mRotation);
if (mMotionProperties != nullptr)
{
if (IsSoftBody())
static_cast<SoftBodyMotionProperties *>(mMotionProperties)->RestoreState(inStream);
else
mMotionProperties->RestoreState(inStream);
JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType);
}
// Initialize bounding box
CalculateWorldSpaceBoundsInternal();
}
BodyCreationSettings Body::GetBodyCreationSettings() const
{
JPH_ASSERT(IsRigidBody());
BodyCreationSettings result;
result.mPosition = GetPosition();
result.mRotation = GetRotation();
result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero();
result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero();
result.mObjectLayer = GetObjectLayer();
result.mUserData = mUserData;
result.mCollisionGroup = GetCollisionGroup();
result.mMotionType = GetMotionType();
result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All;
result.mAllowDynamicOrKinematic = mMotionProperties != nullptr;
result.mIsSensor = IsSensor();
result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic();
result.mUseManifoldReduction = GetUseManifoldReduction();
result.mApplyGyroscopicForce = GetApplyGyroscopicForce();
result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete;
result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval();
result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true;
result.mFriction = GetFriction();
result.mRestitution = GetRestitution();
result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f;
result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f;
result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f;
result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f;
result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f;
result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0;
result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0;
result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
// Invert inertia and mass
if (mMotionProperties != nullptr)
{
float inv_mass = mMotionProperties->GetInverseMassUnchecked();
Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked();
// Get mass
result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX;
// Get inertia
Mat44 inertia;
if (inertia.SetInversed3x3(inv_inertia))
{
// Inertia was invertible, we can use it
result.mMassPropertiesOverride.mInertia = inertia;
}
else
{
// Prevent division by zero
Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN));
result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal());
}
}
else
{
result.mMassPropertiesOverride.mMass = FLT_MAX;
result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX));
}
result.SetShape(GetShape());
return result;
}
SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const
{
JPH_ASSERT(IsSoftBody());
SoftBodyCreationSettings result;
result.mPosition = GetPosition();
result.mRotation = GetRotation();
result.mUserData = mUserData;
result.mObjectLayer = GetObjectLayer();
result.mCollisionGroup = GetCollisionGroup();
result.mFriction = GetFriction();
result.mRestitution = GetRestitution();
const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(mMotionProperties);
result.mNumIterations = mp->GetNumIterations();
result.mLinearDamping = mp->GetLinearDamping();
result.mMaxLinearVelocity = mp->GetMaxLinearVelocity();
result.mGravityFactor = mp->GetGravityFactor();
result.mPressure = mp->GetPressure();
result.mUpdatePosition = mp->GetUpdatePosition();
result.mSettings = mp->GetSettings();
return result;
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,452 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Core/NonCopyable.h>
#include <Jolt/Geometry/AABox.h>
#include <Jolt/Physics/Collision/Shape/Shape.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Collision/CollisionGroup.h>
#include <Jolt/Physics/Collision/TransformedShape.h>
#include <Jolt/Physics/Body/MotionProperties.h>
#include <Jolt/Physics/Body/BodyID.h>
#include <Jolt/Physics/Body/BodyAccess.h>
#include <Jolt/Physics/Body/BodyType.h>
#include <Jolt/Core/StringTools.h>
JPH_NAMESPACE_BEGIN
class StateRecorder;
class BodyCreationSettings;
class SoftBodyCreationSettings;
/// A rigid body that can be simulated using the physics system
///
/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object.
///
/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass().
/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created.
///
/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$.
class
#ifndef JPH_PLATFORM_DOXYGEN // Doxygen gets confused here
JPH_EXPORT_GCC_BUG_WORKAROUND alignas(JPH_RVECTOR_ALIGNMENT)
#endif
Body : public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Get the id of this body
inline const BodyID & GetID() const { return mID; }
/// Get the type of body (rigid or soft)
inline EBodyType GetBodyType() const { return mBodyType; }
/// Check if this body is a rigid body
inline bool IsRigidBody() const { return mBodyType == EBodyType::RigidBody; }
/// Check if this body is a soft body
inline bool IsSoftBody() const { return mBodyType == EBodyType::SoftBody; }
// See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here
JPH_TSAN_NO_SANITIZE
/// If this body is currently actively simulating (true) or sleeping (false)
inline bool IsActive() const { return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; }
/// Check if this body is static (not movable)
inline bool IsStatic() const { return mMotionType == EMotionType::Static; }
/// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it
inline bool IsKinematic() const { return mMotionType == EMotionType::Kinematic; }
/// Check if this body is dynamic, which means that it moves and forces can act on it
inline bool IsDynamic() const { return mMotionType == EMotionType::Dynamic; }
/// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true)
inline bool CanBeKinematicOrDynamic() const { return mMotionProperties != nullptr; }
/// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
/// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation).
/// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost.
/// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically.
/// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase.
inline void SetIsSensor(bool inIsSensor) { JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
/// Check if this body is a sensor.
inline bool IsSensor() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
/// If kinematic objects can generate contact points against other kinematic or static objects.
/// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects).
/// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects.
inline void SetCollideKinematicVsNonDynamic(bool inCollide) { JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); }
/// Check if kinematic objects can generate contact points against other kinematic or static objects.
inline bool GetCollideKinematicVsNonDynamic() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; }
/// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body.
/// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes).
/// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost.
/// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks.
inline void SetUseManifoldReduction(bool inUseReduction) { JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
/// Check if this body can use manifold reduction.
inline bool GetUseManifoldReduction() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; }
/// Checks if the combination of this body and inBody2 should use manifold reduction
inline bool GetUseManifoldReductionWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; }
/// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
inline void SetApplyGyroscopicForce(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); }
/// Check if the gyroscopic force is being applied for this body
inline bool GetApplyGyroscopicForce() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; }
/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
inline void SetEnhancedInternalEdgeRemoval(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); }
/// Check if enhanced internal edge removal is turned on
inline bool GetEnhancedInternalEdgeRemoval() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
/// Checks if the combination of this body and inBody2 should use enhanced internal edge removal
inline bool GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
/// Get the bodies motion type.
inline EMotionType GetMotionType() const { return mMotionType; }
/// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated.
void SetMotionType(EMotionType inMotionType);
/// Get broadphase layer, this determines in which broad phase sub-tree the object is placed
inline BroadPhaseLayer GetBroadPhaseLayer() const { return mBroadPhaseLayer; }
/// Get object layer, this determines which other objects it collides with
inline ObjectLayer GetObjectLayer() const { return mObjectLayer; }
/// Collision group and sub-group ID, determines which other objects it collides with
const CollisionGroup & GetCollisionGroup() const { return mCollisionGroup; }
CollisionGroup & GetCollisionGroup() { return mCollisionGroup; }
void SetCollisionGroup(const CollisionGroup &inGroup) { mCollisionGroup = inGroup; }
/// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up.
bool GetAllowSleeping() const { return mMotionProperties->mAllowSleeping; }
void SetAllowSleeping(bool inAllow);
/// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping.
inline void ResetSleepTimer();
/// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
inline float GetFriction() const { return mFriction; }
void SetFriction(float inFriction) { mFriction = inFriction; }
/// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
inline float GetRestitution() const { return mRestitution; }
void SetRestitution(float inRestitution) { mRestitution = inRestitution; }
/// Get world space linear velocity of the center of mass (unit: m/s)
inline Vec3 GetLinearVelocity() const { return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); }
/// Set world space linear velocity of the center of mass (unit: m/s).
/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); }
/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity.
/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); }
/// Get world space angular velocity of the center of mass (unit: rad/s)
inline Vec3 GetAngularVelocity() const { return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); }
/// Set world space angular velocity of the center of mass (unit: rad/s).
/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); }
/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity.
/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); }
/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
inline Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); }
/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
inline Vec3 GetPointVelocity(RVec3Arg inPoint) const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); }
/// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update.
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
inline void AddForce(Vec3Arg inForce) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
/// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update.
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
inline void AddForce(Vec3Arg inForce, RVec3Arg inPosition);
/// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update.
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddTorque instead.
inline void AddTorque(Vec3Arg inTorque) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
// Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
inline Vec3 GetAccumulatedForce() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); }
// Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
inline Vec3 GetAccumulatedTorque() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); }
// Reset the total accumulated force, not that this will be done automatically after every time step.
JPH_INLINE void ResetForce() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); }
// Reset the total accumulated torque, not that this will be done automatically after every time step.
JPH_INLINE void ResetTorque() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); }
// Reset the current velocity and accumulated force and torque.
JPH_INLINE void ResetMotion() { JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); }
/// Get inverse inertia tensor in world space
inline Mat44 GetInverseInertia() const;
/// Add impulse to center of mass (unit: kg m/s).
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
inline void AddImpulse(Vec3Arg inImpulse);
/// Add impulse to point in world space (unit: kg m/s).
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
inline void AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition);
/// Add angular impulse in world space (unit: N m s).
/// If you want the body to wake up when it is sleeping, use BodyInterface::AddAngularImpulse instead.
inline void AddAngularImpulse(Vec3Arg inAngularImpulse);
/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds.
/// If you want the body to wake up when it is sleeping, use BodyInterface::MoveKinematic instead.
void MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
/// Gets the properties needed to do buoyancy calculations
/// @param inSurfacePosition Position of the fluid surface in world space
/// @param inSurfaceNormal Normal of the fluid surface (should point up)
/// @param outTotalVolume On return this contains the total volume of the shape
/// @param outSubmergedVolume On return this contains the submerged volume of the shape
/// @param outRelativeCenterOfBuoyancy On return this contains the center of mass of the submerged volume relative to the center of mass of the body
void GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const;
/// Applies an impulse to the body that simulates fluid buoyancy and drag.
/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
/// @param inSurfacePosition Position of the fluid surface in world space
/// @param inSurfaceNormal Normal of the fluid surface (should point up)
/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
/// @param inGravity The gravity vector (pointing down)
/// @param inDeltaTime Delta time of the next simulation step (in s)
/// @return true if an impulse was applied, false if the body was not in the fluid
bool ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
/// Applies an impulse to the body that simulates fluid buoyancy and drag.
/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
/// @param inTotalVolume Total volume of the shape of this body (m^3)
/// @param inSubmergedVolume Submerged volume of the shape of this body (m^3)
/// @param inRelativeCenterOfBuoyancy The center of mass of the submerged volume relative to the center of mass of the body
/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
/// @param inGravity The gravity vector (pointing down)
/// @param inDeltaTime Delta time of the next simulation step (in s)
/// @return true if an impulse was applied, false if the body was not in the fluid
bool ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
/// Check if this body has been added to the physics system
inline bool IsInBroadPhase() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; }
/// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body
inline bool IsCollisionCacheInvalid() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; }
/// Get the shape of this body
inline const Shape * GetShape() const { return mShape; }
/// World space position of the body
inline RVec3 GetPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
/// World space rotation of the body
inline Quat GetRotation() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; }
/// Calculates the transform of this body
inline RMat44 GetWorldTransform() const;
/// Gets the world space position of this body's center of mass
inline RVec3 GetCenterOfMassPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; }
/// Calculates the transform for this body's center of mass
inline RMat44 GetCenterOfMassTransform() const;
/// Calculates the inverse of the transform for this body's center of mass
inline RMat44 GetInverseCenterOfMassTransform() const;
/// Get world space bounding box
inline const AABox & GetWorldSpaceBounds() const { return mBounds; }
#ifdef JPH_ENABLE_ASSERTS
/// Validate that the cached bounding box of the body matches the actual bounding box of the body.
/// If this check fails then there are a number of possible causes:
/// 1. Shape is being modified without notifying the system of the change. E.g. if you modify a MutableCompoundShape
/// without calling BodyInterface::NotifyShapeChanged then there will be a mismatch between the cached bounding box
/// in the broad phase and the bounding box of the Shape.
/// 2. You are calling functions postfixed with 'Internal' which are not meant to be called by the application.
/// 3. If the actual bounds and cached bounds are very close, it could mean that you have a mismatch in floating
/// point unit state between threads. E.g. one thread has flush to zero (FTZ) or denormals are zero (DAZ) set and
/// the other thread does not. Or if the rounding mode differs between threads. This can cause small differences
/// in floating point calculations. If you are using JobSystemThreadPool you can use JobSystemThreadPool::SetThreadInitFunction
/// to initialize the floating point unit state.
inline void ValidateCachedBounds() const
{
AABox actual_body_bounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
JPH_ASSERT(actual_body_bounds == mBounds, "Mismatch between cached bounding box and actual bounding box");
}
#endif // JPH_ENABLE_ASSERTS
/// Access to the motion properties
const MotionProperties *GetMotionProperties() const { JPH_ASSERT(!IsStatic()); return mMotionProperties; }
MotionProperties * GetMotionProperties() { JPH_ASSERT(!IsStatic()); return mMotionProperties; }
/// Access to the motion properties (version that does not check if the object is kinematic or dynamic)
const MotionProperties *GetMotionPropertiesUnchecked() const { return mMotionProperties; }
MotionProperties * GetMotionPropertiesUnchecked() { return mMotionProperties; }
/// Access to the user data, can be used for anything by the application
uint64 GetUserData() const { return mUserData; }
void SetUserData(uint64 inUserData) { mUserData = inUserData; }
/// Get surface normal of a particular sub shape and its world space surface position on this body
inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const;
/// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock
inline TransformedShape GetTransformedShape() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); }
/// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later
BodyCreationSettings GetBodyCreationSettings() const;
/// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later
SoftBodyCreationSettings GetSoftBodyCreationSettings() const;
/// A dummy body that can be used by constraints to attach a constraint to the world instead of another body
static Body sFixedToWorld;
///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION
///@{
/// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide
/// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs
static inline bool sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
/// Update position using an Euler step (used during position integrate & constraint solving)
inline void AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
inline void SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
/// Update rotation using an Euler step (used during position integrate & constraint solving)
inline void AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
inline void SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
/// Flag if body is in the broadphase (should only be called by the BroadPhase)
inline void SetInBroadPhaseInternal(bool inInBroadPhase) { if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); }
/// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid.
inline bool InvalidateContactCacheInternal() { return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; }
/// Reset the collision cache invalid flag (should only be called by the BodyManager).
inline void ValidateContactCacheInternal() { JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); }
/// Updates world space bounding box (should only be called by the PhysicsSystem)
void CalculateWorldSpaceBoundsInternal();
/// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase)
void SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true);
/// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase)
/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
void UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties);
/// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase)
/// @param inShape The new shape for this body
/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
void SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties);
// TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair.
// When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will
// then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop.
// This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here.
JPH_TSAN_NO_SANITIZE
/// Access to the index in the BodyManager::mActiveBodies list
uint32 GetIndexInActiveBodiesInternal() const { return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; }
/// Update eligibility for sleeping
ECanSleep UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep);
/// Saving state for replay
void SaveState(StateRecorder &inStream) const;
/// Restoring state for replay
void RestoreState(StateRecorder &inStream);
///@}
static constexpr uint32 cInactiveIndex = MotionProperties::cInactiveIndex; ///< Constant indicating that body is not active
private:
friend class BodyManager;
friend class BodyWithMotionProperties;
friend class SoftBodyWithMotionPropertiesAndShape;
Body() = default; ///< Bodies must be created through BodyInterface::CreateBody
explicit Body(bool); ///< Alternative constructor that initializes all members
~Body() { JPH_ASSERT(mMotionProperties == nullptr); } ///< Bodies must be destroyed through BodyInterface::DestroyBody
inline void GetSleepTestPoints(RVec3 *outPoints) const; ///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
enum class EFlags : uint8
{
IsSensor = 1 << 0, ///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
CollideKinematicVsNonDynamic = 1 << 1, ///< If kinematic objects can generate contact points against other kinematic or static objects.
IsInBroadPhase = 1 << 2, ///< Set this bit to indicate that the body is in the broadphase
InvalidateContactCache = 1 << 3, ///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
UseManifoldReduction = 1 << 4, ///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
ApplyGyroscopicForce = 1 << 5, ///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
EnhancedInternalEdgeRemoval = 1 << 6, ///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval)
};
// 16 byte aligned
RVec3 mPosition; ///< World space position of center of mass
Quat mRotation; ///< World space rotation of center of mass
AABox mBounds; ///< World space bounding box of the body
// 8 byte aligned
RefConst<Shape> mShape; ///< Shape representing the volume of this body
MotionProperties * mMotionProperties = nullptr; ///< If this is a keyframed or dynamic object, this object holds all information about the movement
uint64 mUserData = 0; ///< User data, can be used for anything by the application
CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide)
// 4 byte aligned
float mFriction; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
float mRestitution; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
BodyID mID; ///< ID of the body (index in the bodies array)
// 2 or 4 bytes aligned
ObjectLayer mObjectLayer; ///< The collision layer this body belongs to (determines if two objects can collide)
// 1 byte aligned
EBodyType mBodyType; ///< Type of body (rigid or soft)
BroadPhaseLayer mBroadPhaseLayer; ///< The broad phase layer this body belongs to
EMotionType mMotionType; ///< Type of motion (static, dynamic or kinematic)
atomic<uint8> mFlags = 0; ///< See EFlags for possible flags
// 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
};
static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect");
static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align");
JPH_NAMESPACE_END
#include "Body.inl"

View File

@@ -0,0 +1,197 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
RMat44 Body::GetWorldTransform() const
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
}
RMat44 Body::GetCenterOfMassTransform() const
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
return RMat44::sRotationTranslation(mRotation, mPosition);
}
RMat44 Body::GetInverseCenterOfMassTransform() const
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
return RMat44::sInverseRotationTranslation(mRotation, mPosition);
}
inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
{
// First body should never be a soft body
JPH_ASSERT(!inBody1.IsSoftBody());
// One of these conditions must be true
// - We always allow detecting collisions between kinematic and non-dynamic bodies
// - One of the bodies must be dynamic to collide
// - A kinematic object can collide with a sensor
if (!inBody1.GetCollideKinematicVsNonDynamic()
&& !inBody2.GetCollideKinematicVsNonDynamic()
&& (!inBody1.IsDynamic() && !inBody2.IsDynamic())
&& !(inBody1.IsKinematic() && inBody2.IsSensor())
&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
return false;
// Check that body 1 is active
uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal();
JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active");
// If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice.
// If A is the same body as B we don't want to collide (1)
// If A is dynamic / kinematic and B is static we should collide (2)
// If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if
// - A is active and B is not active (3)
// - A is active and B will become active during this simulation step (4)
// - A is active and B is active, we require a condition that makes A, B collide and B, A not (5)
//
// In order to implement this we use the index in the active body list and make use of the fact that
// a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32.
//
// Because we know that A is active we know that A.Index != 0xffffffff:
// (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index
// (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index
// (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index
// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it
// will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate
// bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
return false;
JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!");
// Check collision group filter
if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup()))
return false;
return true;
}
void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
{
JPH_ASSERT(IsRigidBody());
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
// This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt
// See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints
// URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf
// But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new
// pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it.
// Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized.
float len = inAngularVelocityTimesDeltaTime.Length();
if (len > 1.0e-6f)
{
mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized();
JPH_ASSERT(!mRotation.IsNaN());
}
}
void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
{
JPH_ASSERT(IsRigidBody());
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
// See comment at Body::AddRotationStep
float len = inAngularVelocityTimesDeltaTime.Length();
if (len > 1.0e-6f)
{
mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized();
JPH_ASSERT(!mRotation.IsNaN());
}
}
Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
{
RMat44 inv_com = GetInverseCenterOfMassTransform();
return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized();
}
Mat44 Body::GetInverseInertia() const
{
JPH_ASSERT(IsDynamic());
return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
}
void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition)
{
AddForce(inForce);
AddTorque(Vec3(inPosition - mPosition).Cross(inForce));
}
void Body::AddImpulse(Vec3Arg inImpulse)
{
JPH_ASSERT(IsDynamic());
SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
}
void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
{
JPH_ASSERT(IsDynamic());
SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse)));
}
void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
{
JPH_ASSERT(IsDynamic());
SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse));
}
void Body::GetSleepTestPoints(RVec3 *outPoints) const
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
// Center of mass is the first position
outPoints[0] = mPosition;
// The second and third position are on the largest axis of the bounding box
Vec3 extent = mShape->GetLocalBounds().GetExtent();
int lowest_component = extent.GetLowestComponentIndex();
Mat44 rotation = Mat44::sRotation(mRotation);
switch (lowest_component)
{
case 0:
outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1);
outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
break;
case 1:
outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
break;
case 2:
outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1);
break;
default:
JPH_ASSERT(false);
break;
}
}
void Body::ResetSleepTimer()
{
RVec3 points[3];
GetSleepTestPoints(points);
mMotionProperties->ResetSleepTestSpheres(points);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,68 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#ifdef JPH_ENABLE_ASSERTS
JPH_NAMESPACE_BEGIN
class JPH_EXPORT BodyAccess
{
public:
/// Access rules, used to detect race conditions during simulation
enum class EAccess : uint8
{
None = 0,
Read = 1,
ReadWrite = 3,
};
/// Grant a scope specific access rights on the current thread
class Grant
{
public:
inline Grant(EAccess inVelocity, EAccess inPosition)
{
EAccess &velocity = sVelocityAccess();
EAccess &position = sPositionAccess();
JPH_ASSERT(velocity == EAccess::ReadWrite);
JPH_ASSERT(position == EAccess::ReadWrite);
velocity = inVelocity;
position = inPosition;
}
inline ~Grant()
{
sVelocityAccess() = EAccess::ReadWrite;
sPositionAccess() = EAccess::ReadWrite;
}
};
/// Check if we have permission
static inline bool sCheckRights(EAccess inRights, EAccess inDesiredRights)
{
return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights);
}
/// Access to read/write velocities
static inline EAccess & sVelocityAccess()
{
static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
return sAccess;
}
/// Access to read/write positions
static inline EAccess & sPositionAccess()
{
static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
return sAccess;
}
};
JPH_NAMESPACE_END
#endif // JPH_ENABLE_ASSERTS

View File

@@ -0,0 +1,28 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
class BodyID;
/// A listener class that receives events when a body activates or deactivates.
/// It can be registered with the BodyManager (or PhysicsSystem).
class BodyActivationListener
{
public:
/// Ensure virtual destructor
virtual ~BodyActivationListener() = default;
/// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe.
/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
virtual void OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
/// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe.
/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
virtual void OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,234 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
{
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup)
JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer)
JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType)
JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce)
JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride)
JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier)
JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride)
}
void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mPosition);
inStream.Write(mRotation);
inStream.Write(mLinearVelocity);
inStream.Write(mAngularVelocity);
mCollisionGroup.SaveBinaryState(inStream);
inStream.Write(mObjectLayer);
inStream.Write(mMotionType);
inStream.Write(mAllowedDOFs);
inStream.Write(mAllowDynamicOrKinematic);
inStream.Write(mIsSensor);
inStream.Write(mCollideKinematicVsNonDynamic);
inStream.Write(mUseManifoldReduction);
inStream.Write(mApplyGyroscopicForce);
inStream.Write(mMotionQuality);
inStream.Write(mEnhancedInternalEdgeRemoval);
inStream.Write(mAllowSleeping);
inStream.Write(mFriction);
inStream.Write(mRestitution);
inStream.Write(mLinearDamping);
inStream.Write(mAngularDamping);
inStream.Write(mMaxLinearVelocity);
inStream.Write(mMaxAngularVelocity);
inStream.Write(mGravityFactor);
inStream.Write(mNumVelocityStepsOverride);
inStream.Write(mNumPositionStepsOverride);
inStream.Write(mOverrideMassProperties);
inStream.Write(mInertiaMultiplier);
mMassPropertiesOverride.SaveBinaryState(inStream);
}
void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mPosition);
inStream.Read(mRotation);
inStream.Read(mLinearVelocity);
inStream.Read(mAngularVelocity);
mCollisionGroup.RestoreBinaryState(inStream);
inStream.Read(mObjectLayer);
inStream.Read(mMotionType);
inStream.Read(mAllowedDOFs);
inStream.Read(mAllowDynamicOrKinematic);
inStream.Read(mIsSensor);
inStream.Read(mCollideKinematicVsNonDynamic);
inStream.Read(mUseManifoldReduction);
inStream.Read(mApplyGyroscopicForce);
inStream.Read(mMotionQuality);
inStream.Read(mEnhancedInternalEdgeRemoval);
inStream.Read(mAllowSleeping);
inStream.Read(mFriction);
inStream.Read(mRestitution);
inStream.Read(mLinearDamping);
inStream.Read(mAngularDamping);
inStream.Read(mMaxLinearVelocity);
inStream.Read(mMaxAngularVelocity);
inStream.Read(mGravityFactor);
inStream.Read(mNumVelocityStepsOverride);
inStream.Read(mNumPositionStepsOverride);
inStream.Read(mOverrideMassProperties);
inStream.Read(mInertiaMultiplier);
mMassPropertiesOverride.RestoreBinaryState(inStream);
}
Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings()
{
// If we already have a shape, return it
if (mShapePtr != nullptr)
{
mShape = nullptr;
Shape::ShapeResult result;
result.Set(const_cast<Shape *>(mShapePtr.GetPtr()));
return result;
}
// Check if we have shape settings
if (mShape == nullptr)
{
Shape::ShapeResult result;
result.SetError("No shape present!");
return result;
}
// Create the shape
Shape::ShapeResult result = mShape->Create();
if (result.IsValid())
mShapePtr = result.Get();
mShape = nullptr;
return result;
}
const Shape *BodyCreationSettings::GetShape() const
{
// If we already have a shape, return it
if (mShapePtr != nullptr)
return mShapePtr;
// Check if we have shape settings
if (mShape == nullptr)
return nullptr;
// Create the shape
Shape::ShapeResult result = mShape->Create();
if (result.IsValid())
return result.Get();
Trace("Error: %s", result.GetError().c_str());
JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!");
return nullptr;
}
MassProperties BodyCreationSettings::GetMassProperties() const
{
// Calculate mass properties
MassProperties mass_properties;
switch (mOverrideMassProperties)
{
case EOverrideMassProperties::CalculateMassAndInertia:
mass_properties = GetShape()->GetMassProperties();
mass_properties.mInertia *= mInertiaMultiplier;
mass_properties.mInertia(3, 3) = 1.0f;
break;
case EOverrideMassProperties::CalculateInertia:
mass_properties = GetShape()->GetMassProperties();
mass_properties.ScaleToMass(mMassPropertiesOverride.mMass);
mass_properties.mInertia *= mInertiaMultiplier;
mass_properties.mInertia(3, 3) = 1.0f;
break;
case EOverrideMassProperties::MassAndInertiaProvided:
mass_properties = mMassPropertiesOverride;
break;
}
return mass_properties;
}
void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const
{
// Save creation settings
SaveBinaryState(inStream);
// Save shape
if (ioShapeMap != nullptr && ioMaterialMap != nullptr)
GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap);
else
inStream.Write(~uint32(0));
// Save group filter
StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap);
}
BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap)
{
BCSResult result;
// Read creation settings
BodyCreationSettings settings;
settings.RestoreBinaryState(inStream);
if (inStream.IsEOF() || inStream.IsFailed())
{
result.SetError("Error reading body creation settings");
return result;
}
// Read shape
Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap);
if (shape_result.HasError())
{
result.SetError(shape_result.GetError());
return result;
}
settings.SetShape(shape_result.Get());
// Read group filter
Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap);
if (gfresult.HasError())
{
result.SetError(gfresult.GetError());
return result;
}
settings.mCollisionGroup.SetGroupFilter(gfresult.Get());
result.Set(settings);
return result;
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,124 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Collision/Shape/Shape.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Collision/CollisionGroup.h>
#include <Jolt/Physics/Body/MotionType.h>
#include <Jolt/Physics/Body/MotionQuality.h>
#include <Jolt/Physics/Body/AllowedDOFs.h>
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamUtils.h>
JPH_NAMESPACE_BEGIN
class StreamIn;
class StreamOut;
/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated
enum class EOverrideMassProperties : uint8
{
CalculateMassAndInertia, ///< Tells the system to calculate the mass and inertia based on density
CalculateInertia, ///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass
MassAndInertiaProvided ///< Tells the system to take the mass and inertia from mMassPropertiesOverride
};
/// Settings for constructing a rigid body
class JPH_EXPORT BodyCreationSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings)
public:
/// Constructor
BodyCreationSettings() = default;
BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { }
BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { }
/// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape.
const ShapeSettings * GetShapeSettings() const { return mShape; }
void SetShapeSettings(const ShapeSettings *inShape) { mShape = inShape; mShapePtr = nullptr; }
/// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this.
Shape::ShapeResult ConvertShapeSettings();
/// Access to the run-time shape object. Will convert from ShapeSettings object if needed.
const Shape * GetShape() const;
void SetShape(const Shape *inShape) { mShapePtr = inShape; mShape = nullptr; }
/// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object)
bool HasMassProperties() const { return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; }
/// Calculate (or return when overridden) the mass and inertia for this body
MassProperties GetMassProperties() const;
/// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter.
void SaveBinaryState(StreamOut &inStream) const;
/// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter.
void RestoreBinaryState(StreamIn &inStream);
using GroupFilterToIDMap = StreamUtils::ObjectToIDMap<GroupFilter>;
using IDToGroupFilterMap = StreamUtils::IDToObjectMap<GroupFilter>;
using ShapeToIDMap = Shape::ShapeToIDMap;
using IDToShapeMap = Shape::IDToShapeMap;
using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
/// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
/// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes
/// Pass nullptr to ioGroupFilterMap to skip saving group filters
void SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const;
using BCSResult = Result<BodyCreationSettings>;
/// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
static BCSResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap);
RVec3 mPosition = RVec3::sZero(); ///< Position of the body (not of the center of mass)
Quat mRotation = Quat::sIdentity(); ///< Rotation of the body
Vec3 mLinearVelocity = Vec3::sZero(); ///< World space linear velocity of the center of mass (m/s)
Vec3 mAngularVelocity = Vec3::sZero(); ///< World space angular velocity (rad/s)
/// User data value (can be used by application)
uint64 mUserData = 0;
///@name Collision settings
ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide)
CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide)
///@name Simulation properties
EMotionType mMotionType = EMotionType::Dynamic; ///< Motion type, determines if the object is static, dynamic or kinematic
EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Which degrees of freedom this body has (can be used to limit simulation to 2D)
bool mAllowDynamicOrKinematic = false; ///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic
bool mIsSensor = false; ///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
bool mCollideKinematicVsNonDynamic = false; ///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic.
bool mUseManifoldReduction = true; ///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction)
bool mApplyGyroscopicForce = false; ///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
EMotionQuality mMotionQuality = EMotionQuality::Discrete; ///< Motion quality, or how well it detects collisions when it has a high velocity
bool mEnhancedInternalEdgeRemoval = false; ///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
bool mAllowSleeping = true; ///< If this body can go to sleep or not
float mFriction = 0.2f; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
float mRestitution = 0.0f; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
float mLinearDamping = 0.05f; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
float mAngularDamping = 0.05f; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that this body can reach (m/s)
float mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f; ///< Maximum angular velocity that this body can reach (rad/s)
float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body
uint mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
uint mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
///@name Mass properties of the body (by default calculated by the shape)
EOverrideMassProperties mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used
float mInertiaMultiplier = 1.0f; ///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value
MassProperties mMassPropertiesOverride; ///< Contains replacement mass settings which override the automatically calculated values
private:
/// Collision volume for the body
RefConst<ShapeSettings> mShape; ///< Shape settings, can be serialized. Mutually exclusive with mShapePtr
RefConst<Shape> mShapePtr; ///< Actual shape, cannot be serialized. Mutually exclusive with mShape
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,130 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Core/NonCopyable.h>
#include <Jolt/Physics/Body/BodyID.h>
JPH_NAMESPACE_BEGIN
class Body;
/// Class function to filter out bodies, returns true if test should collide with body
class JPH_EXPORT BodyFilter : public NonCopyable
{
public:
/// Destructor
virtual ~BodyFilter() = default;
/// Filter function. Returns true if we should collide with inBodyID
virtual bool ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const
{
return true;
}
/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
virtual bool ShouldCollideLocked([[maybe_unused]] const Body &inBody) const
{
return true;
}
};
/// A simple body filter implementation that ignores a single, specified body
class JPH_EXPORT IgnoreSingleBodyFilter : public BodyFilter
{
public:
/// Constructor, pass the body you want to ignore
explicit IgnoreSingleBodyFilter(const BodyID &inBodyID) :
mBodyID(inBodyID)
{
}
/// Filter function. Returns true if we should collide with inBodyID
virtual bool ShouldCollide(const BodyID &inBodyID) const override
{
return mBodyID != inBodyID;
}
private:
BodyID mBodyID;
};
/// A simple body filter implementation that ignores multiple, specified bodies
class JPH_EXPORT IgnoreMultipleBodiesFilter : public BodyFilter
{
public:
/// Remove all bodies from the filter
void Clear()
{
mBodyIDs.clear();
}
/// Reserve space for inSize body ID's
void Reserve(uint inSize)
{
mBodyIDs.reserve(inSize);
}
/// Add a body to be ignored
void IgnoreBody(const BodyID &inBodyID)
{
mBodyIDs.push_back(inBodyID);
}
/// Filter function. Returns true if we should collide with inBodyID
virtual bool ShouldCollide(const BodyID &inBodyID) const override
{
return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end();
}
private:
Array<BodyID> mBodyIDs;
};
/// Ignores a single body and chains the filter to another filter
class JPH_EXPORT IgnoreSingleBodyFilterChained : public BodyFilter
{
public:
/// Constructor
explicit IgnoreSingleBodyFilterChained(const BodyID inBodyID, const BodyFilter &inFilter) :
mBodyID(inBodyID),
mFilter(inFilter)
{
}
/// Filter function. Returns true if we should collide with inBodyID
virtual bool ShouldCollide(const BodyID &inBodyID) const override
{
return inBodyID != mBodyID && mFilter.ShouldCollide(inBodyID);
}
/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
virtual bool ShouldCollideLocked(const Body &inBody) const override
{
return mFilter.ShouldCollideLocked(inBody);
}
private:
BodyID mBodyID;
const BodyFilter & mFilter;
};
#ifdef JPH_DEBUG_RENDERER
/// Class function to filter out bodies for debug rendering, returns true if body should be rendered
class JPH_EXPORT BodyDrawFilter : public NonCopyable
{
public:
/// Destructor
virtual ~BodyDrawFilter() = default;
/// Filter function. Returns true if inBody should be rendered
virtual bool ShouldDraw([[maybe_unused]] const Body& inBody) const
{
return true;
}
};
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_END

View File

@@ -0,0 +1,101 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Core/HashCombine.h>
JPH_NAMESPACE_BEGIN
/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions.
class BodyID
{
public:
JPH_OVERRIDE_NEW_DELETE
static constexpr uint32 cInvalidBodyID = 0xffffffff; ///< The value for an invalid body ID
static constexpr uint32 cBroadPhaseBit = 0x80000000; ///< This bit is used by the broadphase
static constexpr uint32 cMaxBodyIndex = 0x7fffff; ///< Maximum value for body index (also the maximum amount of bodies supported - 1)
static constexpr uint8 cMaxSequenceNumber = 0xff; ///< Maximum value for the sequence number
static constexpr uint cSequenceNumberShift = 23; ///< Number of bits to shift to get the sequence number
/// Construct invalid body ID
BodyID() :
mID(cInvalidBodyID)
{
}
/// Construct from index and sequence number combined in a single uint32 (use with care!)
explicit BodyID(uint32 inID) :
mID(inID)
{
JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase
}
/// Construct from index and sequence number
explicit BodyID(uint32 inID, uint8 inSequenceNumber) :
mID((uint32(inSequenceNumber) << cSequenceNumberShift) | inID)
{
JPH_ASSERT(inID <= cMaxBodyIndex); // Should not overlap with broadphase bit or sequence number
}
/// Get index in body array
inline uint32 GetIndex() const
{
return mID & cMaxBodyIndex;
}
/// Get sequence number of body.
/// The sequence number can be used to check if a body ID with the same body index has been reused by another body.
/// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread.
/// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row).
inline uint8 GetSequenceNumber() const
{
return uint8(mID >> cSequenceNumberShift);
}
/// Returns the index and sequence number combined in an uint32
inline uint32 GetIndexAndSequenceNumber() const
{
return mID;
}
/// Check if the ID is valid
inline bool IsInvalid() const
{
return mID == cInvalidBodyID;
}
/// Equals check
inline bool operator == (const BodyID &inRHS) const
{
return mID == inRHS.mID;
}
/// Not equals check
inline bool operator != (const BodyID &inRHS) const
{
return mID != inRHS.mID;
}
/// Smaller than operator, can be used for sorting bodies
inline bool operator < (const BodyID &inRHS) const
{
return mID < inRHS.mID;
}
/// Greater than operator, can be used for sorting bodies
inline bool operator > (const BodyID &inRHS) const
{
return mID > inRHS.mID;
}
private:
uint32 mID;
};
JPH_NAMESPACE_END
// Create a std::hash/JPH::Hash for BodyID
JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber())

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,305 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/BodyID.h>
#include <Jolt/Physics/EActivation.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Body/MotionType.h>
#include <Jolt/Physics/Body/MotionQuality.h>
#include <Jolt/Physics/Body/BodyType.h>
#include <Jolt/Core/Reference.h>
JPH_NAMESPACE_BEGIN
class Body;
class BodyCreationSettings;
class SoftBodyCreationSettings;
class BodyLockInterface;
class BroadPhase;
class BodyManager;
class TransformedShape;
class PhysicsMaterial;
class SubShapeID;
class Shape;
class TwoBodyConstraintSettings;
class TwoBodyConstraint;
class BroadPhaseLayerFilter;
class AABox;
class CollisionGroup;
/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
/// All quantities are in world space unless otherwise specified.
class JPH_EXPORT BodyInterface : public NonCopyable
{
public:
/// Initialize the interface (should only be called by PhysicsSystem)
void Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; }
/// Create a rigid body
/// @return Created body or null when out of bodies
Body * CreateBody(const BodyCreationSettings &inSettings);
/// Create a soft body
/// @return Created body or null when out of bodies
Body * CreateSoftBody(const SoftBodyCreationSettings &inSettings);
/// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
/// The ID created on the server can be replicated to the client and used to create a deterministic simulation.
/// @return Created body or null when the body ID is invalid or a body of the same ID already exists.
Body * CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings);
/// Create a soft body with specified ID. See comments at CreateBodyWithID.
Body * CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings);
/// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
/// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID.
/// @return Created body
Body * CreateBodyWithoutID(const BodyCreationSettings &inSettings) const;
/// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID.
Body * CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const;
/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function,
/// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody.
void DestroyBodyWithoutID(Body *inBody) const;
/// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
/// @return false if the body already has an ID or out of body ids.
bool AssignBodyID(Body *ioBody);
/// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
/// @return false if the body already has an ID or if the ID is not valid.
bool AssignBodyID(Body *ioBody, const BodyID &inBodyID);
/// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body.
Body * UnassignBodyID(const BodyID &inBodyID);
/// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system.
/// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies.
/// @param inBodyIDs A list of body IDs
/// @param inNumber Number of bodies in the list
/// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these).
void UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
/// Destroy a body.
/// Make sure that you remove the body from the physics system using BodyInterface::RemoveBody before calling this function.
void DestroyBody(const BodyID &inBodyID);
/// Destroy multiple bodies
/// Make sure that you remove the bodies from the physics system using BodyInterface::RemoveBody before calling this function.
void DestroyBodies(const BodyID *inBodyIDs, int inNumber);
/// Add body to the physics system.
/// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function.
/// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree!
/// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface!
void AddBody(const BodyID &inBodyID, EActivation inActivationMode);
/// Remove body from the physics system.
void RemoveBody(const BodyID &inBodyID);
/// Check if a body has been added to the physics system.
bool IsAdded(const BodyID &inBodyID) const;
/// Combines CreateBody and AddBody
/// @return Created body ID or an invalid ID when out of bodies
BodyID CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode);
/// Combines CreateSoftBody and AddBody
/// @return Created body ID or an invalid ID when out of bodies
BodyID CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode);
/// Add state handle, used to keep track of a batch of bodies while adding them to the PhysicsSystem.
using AddState = void *;
///@name Batch adding interface
///@{
/// Prepare adding inNumber bodies at ioBodies to the PhysicsSystem, returns a handle that should be used in AddBodiesFinalize/Abort.
/// This can be done on a background thread without influencing the PhysicsSystem.
/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber);
/// Finalize adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode);
/// Abort adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
/// This can be done on a background thread without influencing the PhysicsSystem.
/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState);
/// Remove inNumber bodies in ioBodies from the PhysicsSystem.
/// ioBodies may be shuffled around by this function.
void RemoveBodies(BodyID *ioBodies, int inNumber);
///@}
///@name Activate / deactivate a body
///@{
void ActivateBody(const BodyID &inBodyID);
void ActivateBodies(const BodyID *inBodyIDs, int inNumber);
void ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter);
void DeactivateBody(const BodyID &inBodyID);
void DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
bool IsActive(const BodyID &inBodyID) const;
void ResetSleepTimer(const BodyID &inBodyID);
///@}
/// Create a two body constraint
TwoBodyConstraint * CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2);
/// Activate non-static bodies attached to a constraint
void ActivateConstraint(const TwoBodyConstraint *inConstraint);
///@name Access to the shape of a body
///@{
/// Get the current shape
RefConst<Shape> GetShape(const BodyID &inBodyID) const;
/// Set a new shape on the body
/// @param inBodyID Body ID of body that had its shape changed
/// @param inShape The new shape
/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
/// @param inActivationMode Whether or not to activate the body
void SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const;
/// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes)
/// @param inBodyID Body ID of body that had its shape changed
/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
/// @param inActivationMode Whether or not to activate the body
void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const;
///@}
///@name Object layer of a body
///@{
void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer);
ObjectLayer GetObjectLayer(const BodyID &inBodyID) const;
///@}
///@name Position and rotation of a body
///@{
void SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode);
void SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change.
void GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const;
void SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode);
RVec3 GetPosition(const BodyID &inBodyID) const;
RVec3 GetCenterOfMassPosition(const BodyID &inBodyID) const;
void SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode);
Quat GetRotation(const BodyID &inBodyID) const;
RMat44 GetWorldTransform(const BodyID &inBodyID) const;
RMat44 GetCenterOfMassTransform(const BodyID &inBodyID) const;
///@}
/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed)
void MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
/// Linear or angular velocity (functions will activate body if needed).
/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
void SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
void GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
void SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity);
Vec3 GetLinearVelocity(const BodyID &inBodyID) const;
void AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity
void AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities
void SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity);
Vec3 GetAngularVelocity(const BodyID &inBodyID) const;
Vec3 GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body
/// Set the complete motion state of a body.
/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
void SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
///@name Add forces to the body
///@{
void AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce
void AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint
void AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque
void AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque
///@}
///@name Add an impulse to the body
///@{
void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass
void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint
void AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse);
bool ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
///@}
///@name Body type
///@{
EBodyType GetBodyType(const BodyID &inBodyID) const;
///@}
///@name Body motion type
///@{
void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode);
EMotionType GetMotionType(const BodyID &inBodyID) const;
///@}
///@name Body motion quality
///@{
void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality);
EMotionQuality GetMotionQuality(const BodyID &inBodyID) const;
///@}
/// Get inverse inertia tensor in world space
Mat44 GetInverseInertia(const BodyID &inBodyID) const;
///@name Restitution
///@{
void SetRestitution(const BodyID &inBodyID, float inRestitution);
float GetRestitution(const BodyID &inBodyID) const;
///@}
///@name Friction
///@{
void SetFriction(const BodyID &inBodyID, float inFriction);
float GetFriction(const BodyID &inBodyID) const;
///@}
///@name Gravity factor
///@{
void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor);
float GetGravityFactor(const BodyID &inBodyID) const;
///@}
///@name Manifold reduction
///@{
void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction);
bool GetUseManifoldReduction(const BodyID &inBodyID) const;
///@}
///@name Collision group
///@{
void SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup);
const CollisionGroup & GetCollisionGroup(const BodyID &inBodyID) const;
///@}
/// Get transform and shape for this body, used to perform collision detection
TransformedShape GetTransformedShape(const BodyID &inBodyID) const;
/// Get the user data for a body
uint64 GetUserData(const BodyID &inBodyID) const;
void SetUserData(const BodyID &inBodyID, uint64 inUserData) const;
/// Get the material for a particular sub shape
const PhysicsMaterial * GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const;
/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
void InvalidateContactCache(const BodyID &inBodyID);
private:
/// Helper function to activate a single body
JPH_INLINE void ActivateBodyInternal(Body &ioBody) const;
BodyLockInterface * mBodyLockInterface = nullptr;
BodyManager * mBodyManager = nullptr;
BroadPhase * mBroadPhase = nullptr;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,111 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/BodyLockInterface.h>
JPH_NAMESPACE_BEGIN
/// Base class for locking bodies for the duration of the scope of this class (do not use directly)
template <bool Write, class BodyType>
class BodyLockBase : public NonCopyable
{
public:
/// Constructor will lock the body
BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) :
mBodyLockInterface(inBodyLockInterface)
{
if (inBodyID == BodyID())
{
// Invalid body id
mBodyLockMutex = nullptr;
mBody = nullptr;
}
else
{
// Get mutex
mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID);
// Get a reference to the body or nullptr when it is no longer valid
mBody = inBodyLockInterface.TryGetBody(inBodyID);
}
}
/// Explicitly release the lock (normally this is done in the destructor)
inline void ReleaseLock()
{
if (mBodyLockMutex != nullptr)
{
if (Write)
mBodyLockInterface.UnlockWrite(mBodyLockMutex);
else
mBodyLockInterface.UnlockRead(mBodyLockMutex);
mBodyLockMutex = nullptr;
mBody = nullptr;
}
}
/// Destructor will unlock the body
~BodyLockBase()
{
ReleaseLock();
}
/// Test if the lock was successful (if the body ID was valid)
inline bool Succeeded() const
{
return mBody != nullptr;
}
/// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase
inline bool SucceededAndIsInBroadPhase() const
{
return mBody != nullptr && mBody->IsInBroadPhase();
}
/// Access the body
inline BodyType & GetBody() const
{
JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first");
return *mBody;
}
private:
const BodyLockInterface & mBodyLockInterface;
SharedMutex * mBodyLockMutex;
BodyType * mBody;
};
/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members
///
/// The common usage pattern is:
///
/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
/// BodyID body_id = ...; // Obtain ID to body
///
/// // Scoped lock
/// {
/// BodyLockRead lock(lock_interface, body_id);
/// if (lock.Succeeded()) // body_id may no longer be valid
/// {
/// const Body &body = lock.GetBody();
///
/// // Do something with body
/// ...
/// }
/// }
class BodyLockRead : public BodyLockBase<false, const Body>
{
using BodyLockBase::BodyLockBase;
};
/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns.
class BodyLockWrite : public BodyLockBase<true, Body>
{
using BodyLockBase::BodyLockBase;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,134 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/Body/BodyManager.h>
#include <Jolt/Physics/PhysicsLock.h>
#include <Jolt/Core/Mutex.h>
JPH_NAMESPACE_BEGIN
/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead.
class BodyLockInterface : public NonCopyable
{
public:
/// Redefine MutexMask
using MutexMask = BodyManager::MutexMask;
/// Constructor
explicit BodyLockInterface(BodyManager &inBodyManager) : mBodyManager(inBodyManager) { }
virtual ~BodyLockInterface() = default;
///@name Locking functions
///@{
virtual SharedMutex * LockRead(const BodyID &inBodyID) const = 0;
virtual void UnlockRead(SharedMutex *inMutex) const = 0;
virtual SharedMutex * LockWrite(const BodyID &inBodyID) const = 0;
virtual void UnlockWrite(SharedMutex *inMutex) const = 0;
///@}
/// Get the mask needed to lock all bodies
inline MutexMask GetAllBodiesMutexMask() const
{
return mBodyManager.GetAllBodiesMutexMask();
}
///@name Batch locking functions
///@{
virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const = 0;
virtual void LockRead(MutexMask inMutexMask) const = 0;
virtual void UnlockRead(MutexMask inMutexMask) const = 0;
virtual void LockWrite(MutexMask inMutexMask) const = 0;
virtual void UnlockWrite(MutexMask inMutexMask) const = 0;
///@}
/// Convert body ID to body
inline Body * TryGetBody(const BodyID &inBodyID) const { return mBodyManager.TryGetBody(inBodyID); }
protected:
BodyManager & mBodyManager;
};
/// Implementation that performs no locking (assumes the lock has already been taken)
class BodyLockInterfaceNoLock final : public BodyLockInterface
{
public:
using BodyLockInterface::BodyLockInterface;
///@name Locking functions
virtual SharedMutex * LockRead([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; }
virtual void UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ }
virtual SharedMutex * LockWrite([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; }
virtual void UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ }
///@name Batch locking functions
virtual MutexMask GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; }
virtual void LockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ }
virtual void UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ }
virtual void LockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ }
virtual void UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ }
};
/// Implementation that uses the body manager to lock the correct mutex for a body
class BodyLockInterfaceLocking final : public BodyLockInterface
{
public:
using BodyLockInterface::BodyLockInterface;
///@name Locking functions
virtual SharedMutex * LockRead(const BodyID &inBodyID) const override
{
SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
return &mutex;
}
virtual void UnlockRead(SharedMutex *inMutex) const override
{
PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
}
virtual SharedMutex * LockWrite(const BodyID &inBodyID) const override
{
SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
return &mutex;
}
virtual void UnlockWrite(SharedMutex *inMutex) const override
{
PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
}
///@name Batch locking functions
virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const override
{
return mBodyManager.GetMutexMask(inBodies, inNumber);
}
virtual void LockRead(MutexMask inMutexMask) const override
{
mBodyManager.LockRead(inMutexMask);
}
virtual void UnlockRead(MutexMask inMutexMask) const override
{
mBodyManager.UnlockRead(inMutexMask);
}
virtual void LockWrite(MutexMask inMutexMask) const override
{
mBodyManager.LockWrite(inMutexMask);
}
virtual void UnlockWrite(MutexMask inMutexMask) const override
{
mBodyManager.UnlockWrite(inMutexMask);
}
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,104 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/BodyLockInterface.h>
JPH_NAMESPACE_BEGIN
/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly)
template <bool Write, class BodyType>
class BodyLockMultiBase : public NonCopyable
{
public:
/// Redefine MutexMask
using MutexMask = BodyLockInterface::MutexMask;
/// Constructor will lock the bodies
BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) :
mBodyLockInterface(inBodyLockInterface),
mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)),
mBodyIDs(inBodyIDs),
mNumBodyIDs(inNumber)
{
if (mMutexMask != 0)
{
// Get mutex
if (Write)
inBodyLockInterface.LockWrite(mMutexMask);
else
inBodyLockInterface.LockRead(mMutexMask);
}
}
/// Destructor will unlock the bodies
~BodyLockMultiBase()
{
if (mMutexMask != 0)
{
if (Write)
mBodyLockInterface.UnlockWrite(mMutexMask);
else
mBodyLockInterface.UnlockRead(mMutexMask);
}
}
/// Access the body (returns null if body was not properly locked)
inline BodyType * GetBody(int inBodyIndex) const
{
// Range check
JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs);
// Get body ID
const BodyID &body_id = mBodyIDs[inBodyIndex];
if (body_id.IsInvalid())
return nullptr;
// Get a reference to the body or nullptr when it is no longer valid
return mBodyLockInterface.TryGetBody(body_id);
}
private:
const BodyLockInterface & mBodyLockInterface;
MutexMask mMutexMask;
const BodyID * mBodyIDs;
int mNumBodyIDs;
};
/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members
///
/// The common usage pattern is:
///
/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
/// const BodyID *body_id = ...; // Obtain IDs to bodies
/// int num_body_ids = ...;
///
/// // Scoped lock
/// {
/// BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids);
/// for (int i = 0; i < num_body_ids; ++i)
/// {
/// const Body *body = lock.GetBody(i);
/// if (body != nullptr)
/// {
/// const Body &body = lock.Body();
///
/// // Do something with body
/// ...
/// }
/// }
/// }
class BodyLockMultiRead : public BodyLockMultiBase<false, const Body>
{
using BodyLockMultiBase::BodyLockMultiBase;
};
/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns.
class BodyLockMultiWrite : public BodyLockMultiBase<true, Body>
{
using BodyLockMultiBase::BodyLockMultiBase;
};
JPH_NAMESPACE_END

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,377 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Core/Mutex.h>
#include <Jolt/Core/MutexArray.h>
JPH_NAMESPACE_BEGIN
// Classes
class BodyCreationSettings;
class SoftBodyCreationSettings;
class BodyActivationListener;
class StateRecorderFilter;
struct PhysicsSettings;
#ifdef JPH_DEBUG_RENDERER
class DebugRenderer;
class BodyDrawFilter;
#endif // JPH_DEBUG_RENDERER
#ifdef JPH_DEBUG_RENDERER
/// Defines how to color soft body constraints
enum class ESoftBodyConstraintColor
{
ConstraintType, /// Draw different types of constraints in different colors
ConstraintGroup, /// Draw constraints in the same group in the same color, non-parallel group will be red
ConstraintOrder, /// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient
};
#endif // JPH_DEBUG_RENDERER
/// Array of bodies
using BodyVector = Array<Body *>;
/// Array of body ID's
using BodyIDVector = Array<BodyID>;
/// Class that contains all bodies
class JPH_EXPORT BodyManager : public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Destructor
~BodyManager();
/// Initialize the manager
void Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface);
/// Gets the current amount of bodies that are in the body manager
uint GetNumBodies() const;
/// Gets the max bodies that we can support
uint GetMaxBodies() const { return uint(mBodies.capacity()); }
/// Helper struct that counts the number of bodies of each type
struct BodyStats
{
uint mNumBodies = 0; ///< Total number of bodies in the body manager
uint mMaxBodies = 0; ///< Max allowed number of bodies in the body manager (as configured in Init(...))
uint mNumBodiesStatic = 0; ///< Number of static bodies
uint mNumBodiesDynamic = 0; ///< Number of dynamic bodies
uint mNumActiveBodiesDynamic = 0; ///< Number of dynamic bodies that are currently active
uint mNumBodiesKinematic = 0; ///< Number of kinematic bodies
uint mNumActiveBodiesKinematic = 0; ///< Number of kinematic bodies that are currently active
uint mNumSoftBodies = 0; ///< Number of soft bodies
uint mNumActiveSoftBodies = 0; ///< Number of soft bodies that are currently active
};
/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
BodyStats GetBodyStats() const;
/// Create a body using creation settings. The returned body will not be part of the body manager yet.
Body * AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
Body * AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
void FreeBody(Body *inBody) const;
/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
bool AddBody(Body *ioBody);
/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
bool AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
/// Remove a list of bodies from the body manager
void RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
/// Remove a set of bodies from the body manager and frees them.
void DestroyBodies(const BodyID *inBodyIDs, int inNumber);
/// Activate a list of bodies.
/// This function should only be called when an exclusive lock for the bodies are held.
void ActivateBodies(const BodyID *inBodyIDs, int inNumber);
/// Deactivate a list of bodies.
/// This function should only be called when an exclusive lock for the bodies are held.
void DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
/// Update the motion quality for a body
void SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
/// Get copy of the list of active bodies under protection of a lock.
void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; }
/// Get the number of active bodies.
uint32 GetNumActiveBodies(EBodyType inType) const { return mNumActiveBodies[int(inType)].load(memory_order_acquire); }
/// Get the number of active bodies that are using continuous collision detection
uint32 GetNumActiveCCDBodies() const { return mNumActiveCCDBodies; }
/// Listener that is notified whenever a body is activated/deactivated
void SetBodyActivationListener(BodyActivationListener *inListener);
BodyActivationListener * GetBodyActivationListener() const { return mActivationListener; }
/// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist.
static inline bool sIsValidBodyPointer(const Body *inBody) { return (uintptr_t(inBody) & cIsFreedBody) == 0; }
/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
const BodyVector & GetBodies() const { return mBodies; }
/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
BodyVector & GetBodies() { return mBodies; }
/// Get all body IDs under the protection of a lock
void GetBodyIDs(BodyIDVector &outBodies) const;
/// Access a body (not protected by lock)
const Body & GetBody(const BodyID &inID) const { return *mBodies[inID.GetIndex()]; }
/// Access a body (not protected by lock)
Body & GetBody(const BodyID &inID) { return *mBodies[inID.GetIndex()]; }
/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
const Body * TryGetBody(const BodyID &inID) const
{
uint32 idx = inID.GetIndex();
if (idx >= mBodies.size())
return nullptr;
const Body *body = mBodies[idx];
if (sIsValidBodyPointer(body) && body->GetID() == inID)
return body;
return nullptr;
}
/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
Body * TryGetBody(const BodyID &inID)
{
uint32 idx = inID.GetIndex();
if (idx >= mBodies.size())
return nullptr;
Body *body = mBodies[idx];
if (sIsValidBodyPointer(body) && body->GetID() == inID)
return body;
return nullptr;
}
/// Access the mutex for a single body
SharedMutex & GetMutexForBody(const BodyID &inID) const { return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); }
/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
using MutexMask = uint64;
///@name Batch body mutex access (do not use directly)
///@{
MutexMask GetAllBodiesMutexMask() const { return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const;
void LockRead(MutexMask inMutexMask) const;
void UnlockRead(MutexMask inMutexMask) const;
void LockWrite(MutexMask inMutexMask) const;
void UnlockWrite(MutexMask inMutexMask) const;
///@}
/// Lock all bodies. This should only be done during PhysicsSystem::Update().
void LockAllBodies() const;
/// Unlock all bodies. This should only be done during PhysicsSystem::Update().
void UnlockAllBodies() const;
/// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase)
inline void SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); }
/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
void InvalidateContactCacheForBody(Body &ioBody);
/// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again.
void ValidateContactCacheForAllBodies();
/// Saving state for replay
void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
/// Restoring state for replay. Returns false if failed.
bool RestoreState(StateRecorder &inStream);
/// Save the state of a single body for replay
void SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
/// Save the state of a single body for replay
void RestoreBodyState(Body &inBody, StateRecorder &inStream);
#ifdef JPH_DEBUG_RENDERER
enum class EShapeColor
{
InstanceColor, ///< Random color per instance
ShapeTypeColor, ///< Convex = green, scaled = yellow, compound = orange, mesh = red
MotionTypeColor, ///< Static = grey, keyframed = green, dynamic = random color per instance
SleepColor, ///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red
IslandColor, ///< Static = grey, active = random color per island, sleeping = light grey
MaterialColor, ///< Color as defined by the PhysicsMaterial of the shape
};
/// Draw settings
struct DrawSettings
{
bool mDrawGetSupportFunction = false; ///< Draw the GetSupport() function, used for convex collision detection
bool mDrawSupportDirection = false; ///< When drawing the support function, also draw which direction mapped to a specific support point
bool mDrawGetSupportingFace = false; ///< Draw the faces that were found colliding during collision detection
bool mDrawShape = true; ///< Draw the shapes of all bodies
bool mDrawShapeWireframe = false; ///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid.
EShapeColor mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes
bool mDrawBoundingBox = false; ///< Draw a bounding box per body
bool mDrawCenterOfMassTransform = false; ///< Draw the center of mass for each body
bool mDrawWorldTransform = false; ///< Draw the world transform (which can be different than the center of mass) for each body
bool mDrawVelocity = false; ///< Draw the velocity vector for each body
bool mDrawMassAndInertia = false; ///< Draw the mass and inertia (as the box equivalent) for each body
bool mDrawSleepStats = false; ///< Draw stats regarding the sleeping algorithm of each body
bool mDrawSoftBodyVertices = false; ///< Draw the vertices of soft bodies
bool mDrawSoftBodyVertexVelocities = false; ///< Draw the velocities of the vertices of soft bodies
bool mDrawSoftBodyEdgeConstraints = false; ///< Draw the edge constraints of soft bodies
bool mDrawSoftBodyBendConstraints = false; ///< Draw the bend constraints of soft bodies
bool mDrawSoftBodyVolumeConstraints = false; ///< Draw the volume constraints of soft bodies
bool mDrawSoftBodySkinConstraints = false; ///< Draw the skin constraints of soft bodies
bool mDrawSoftBodyLRAConstraints = false; ///< Draw the LRA constraints of soft bodies
bool mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies
ESoftBodyConstraintColor mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
};
/// Draw the state of the bodies (debugging purposes)
void Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr);
#endif // JPH_DEBUG_RENDERER
#ifdef JPH_ENABLE_ASSERTS
/// Lock the active body list, asserts when Activate/DeactivateBody is called.
void SetActiveBodiesLocked(bool inLocked) { mActiveBodiesLocked = inLocked; }
/// Per thread override of the locked state, to be used by the PhysicsSystem only!
class GrantActiveBodiesAccess
{
public:
inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation)
{
JPH_ASSERT(!sGetOverrideAllowActivation());
sSetOverrideAllowActivation(inAllowActivation);
JPH_ASSERT(!sGetOverrideAllowDeactivation());
sSetOverrideAllowDeactivation(inAllowDeactivation);
}
inline ~GrantActiveBodiesAccess()
{
sSetOverrideAllowActivation(false);
sSetOverrideAllowDeactivation(false);
}
};
#endif
#ifdef JPH_DEBUG
/// Validate if the cached bounding boxes are correct for all active bodies
void ValidateActiveBodyBounds();
#endif // JPH_DEBUG
private:
/// Increment and get the sequence number of the body
#ifdef JPH_COMPILER_CLANG
__attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number
#endif
inline uint8 GetNextSequenceNumber(int inBodyIndex) { return ++mBodySequenceNumbers[inBodyIndex]; }
/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
inline void AddBodyToActiveBodies(Body &ioBody);
/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
inline void RemoveBodyFromActiveBodies(Body &ioBody);
/// Helper function to remove a body from the manager
JPH_INLINE Body * RemoveBodyInternal(const BodyID &inBodyID);
/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
inline static void sDeleteBody(Body *inBody);
#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
/// Function to check that the free list is not corrupted
void ValidateFreeList() const;
#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
BodyVector mBodies;
/// Current number of allocated bodies
uint mNumBodies = 0;
/// Indicates that there are no more freed body IDs
static constexpr uintptr_t cBodyIDFreeListEnd = ~uintptr_t(0);
/// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit.
static constexpr uintptr_t cIsFreedBody = uintptr_t(1);
/// Amount of bits to shift to get an index to the next freed body
static constexpr uint cFreedBodyIndexShift = 1;
/// Index of first entry in mBodies that is unused
uintptr_t mBodyIDFreeListStart = cBodyIDFreeListEnd;
/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
mutable Mutex mBodiesMutex;
/// An array of mutexes protecting the bodies in the mBodies array
using BodyMutexes = MutexArray<SharedMutex>;
mutable BodyMutexes mBodyMutexes;
/// List of next sequence number for a body ID
Array<uint8> mBodySequenceNumbers;
/// Mutex that protects the mActiveBodies array
mutable Mutex mActiveBodiesMutex;
/// List of all active dynamic bodies (size is equal to max amount of bodies)
BodyID * mActiveBodies[cBodyTypeCount] = { };
/// How many bodies there are in the list of active bodies
atomic<uint32> mNumActiveBodies[cBodyTypeCount] = { };
/// How many of the active bodies have continuous collision detection enabled
uint32 mNumActiveCCDBodies = 0;
/// Mutex that protects the mBodiesCacheInvalid array
mutable Mutex mBodiesCacheInvalidMutex;
/// List of all bodies that should have their cache invalidated
BodyIDVector mBodiesCacheInvalid;
/// Listener that is notified whenever a body is activated/deactivated
BodyActivationListener * mActivationListener = nullptr;
/// Cached broadphase layer interface
const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
#ifdef JPH_ENABLE_ASSERTS
static bool sGetOverrideAllowActivation();
static void sSetOverrideAllowActivation(bool inValue);
static bool sGetOverrideAllowDeactivation();
static void sSetOverrideAllowDeactivation(bool inValue);
/// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update()
bool mActiveBodiesLocked = false;
#endif
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,36 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/BodyID.h>
#include <Jolt/Core/HashCombine.h>
JPH_NAMESPACE_BEGIN
/// Structure that holds a body pair
struct alignas(uint64) BodyPair
{
JPH_OVERRIDE_NEW_DELETE
/// Constructor
BodyPair() = default;
BodyPair(BodyID inA, BodyID inB) : mBodyA(inA), mBodyB(inB) { }
/// Equals operator
bool operator == (const BodyPair &inRHS) const { return *reinterpret_cast<const uint64 *>(this) == *reinterpret_cast<const uint64 *>(&inRHS); }
/// Smaller than operator, used for consistently ordering body pairs
bool operator < (const BodyPair &inRHS) const { return *reinterpret_cast<const uint64 *>(this) < *reinterpret_cast<const uint64 *>(&inRHS); }
/// Get the hash value of this object
uint64 GetHash() const { return Hash64(*reinterpret_cast<const uint64 *>(this)); }
BodyID mBodyA;
BodyID mBodyB;
};
static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size");
JPH_NAMESPACE_END

View File

@@ -0,0 +1,19 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
/// Type of body
enum class EBodyType : uint8
{
RigidBody, ///< Rigid body consisting of a rigid shape
SoftBody, ///< Soft body consisting of a deformable shape
};
/// How many types of bodies there are
static constexpr uint cBodyTypeCount = 2;
JPH_NAMESPACE_END

View File

@@ -0,0 +1,185 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Body/MassProperties.h>
#include <Jolt/Math/Matrix.h>
#include <Jolt/Math/Vector.h>
#include <Jolt/Math/EigenValueSymmetric.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Core/InsertionSort.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties)
{
JPH_ADD_ATTRIBUTE(MassProperties, mMass)
JPH_ADD_ATTRIBUTE(MassProperties, mInertia)
}
bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const
{
// Using eigendecomposition to get the principal components of the inertia tensor
// See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix
Matrix<3, 3> inertia;
inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0);
Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity();
Vector<3> eigen_val;
if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val))
return false;
// Sort so that the biggest value goes first
int indices[] = { 0, 1, 2 };
InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
// Convert to a regular Mat44 and Vec3
outRotation = Mat44::sIdentity();
for (int i = 0; i < 3; ++i)
{
outRotation.SetColumn3(i, Vec3(reinterpret_cast<Float3 &>(eigen_vec.GetColumn(indices[i]))));
outDiagonal.SetComponent(i, eigen_val[indices[i]]);
}
// Make sure that the rotation matrix is a right handed matrix
if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f)
outRotation.SetAxisZ(-outRotation.GetAxisZ());
#ifdef JPH_ENABLE_ASSERTS
// Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is
// smaller than some fraction of the inertia itself in that axis
Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed();
for (int i = 0; i < 3; ++i)
JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f));
#endif
return true;
}
void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity)
{
// Calculate mass
mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity;
// Calculate inertia
Vec3 size_sq = inBoxSize * inBoxSize;
Vec3 scale = (size_sq.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() + size_sq.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>()) * (mMass / 12.0f);
mInertia = Mat44::sScale(scale);
}
void MassProperties::ScaleToMass(float inMass)
{
if (mMass > 0.0f)
{
// Calculate how much we have to scale the inertia tensor
float mass_scale = inMass / mMass;
// Update mass
mMass = inMass;
// Update inertia tensor
for (int i = 0; i < 3; ++i)
mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale);
}
else
{
// Just set the mass
mMass = inMass;
}
}
Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal)
{
// Moment of inertia of a solid box has diagonal:
// mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2]
// Solving for size_x, size_y and size_y (diagonal and mass are known):
Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass);
return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2])));
}
void MassProperties::Scale(Vec3Arg inScale)
{
// See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
// The diagonal of the inertia tensor can be calculated like this:
// Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2))
// Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2))
// Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2))
//
// We want to isolate the terms x_k, y_k and z_k:
// d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz]
// [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz]
Vec3 diagonal = mInertia.GetDiagonal3();
Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal;
// When scaling a shape these terms change like this:
// sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2)
// Same for y_k and z_k
// Using these terms we can calculate the new diagonal of the inertia tensor:
Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq;
float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ();
float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ();
float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY();
// The off diagonal elements are calculated like:
// Ixy = -sum_{k = 1 to n}(x_k y_k)
// Ixz = -sum_{k = 1 to n}(x_k z_k)
// Iyz = -sum_{k = 1 to n}(y_k z_k)
// Scaling these is simple:
float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1);
float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2);
float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2);
// Update inertia tensor
mInertia(0, 0) = i_xx;
mInertia(0, 1) = i_xy;
mInertia(1, 0) = i_xy;
mInertia(1, 1) = i_yy;
mInertia(0, 2) = i_xz;
mInertia(2, 0) = i_xz;
mInertia(1, 2) = i_yz;
mInertia(2, 1) = i_yz;
mInertia(2, 2) = i_zz;
// Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative)
float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ());
mMass *= mass_scale;
// Inertia scales linear with mass. This updates the m_k terms above.
mInertia *= mass_scale;
// Ensure that the bottom right element is a 1 again
mInertia(3, 3) = 1.0f;
}
void MassProperties::Rotate(Mat44Arg inRotation)
{
mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation);
}
void MassProperties::Translate(Vec3Arg inTranslation)
{
// Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T)
// Where I is the original body's inertia and E the identity matrix
// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem
mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation));
// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
}
void MassProperties::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mMass);
inStream.Write(mInertia);
}
void MassProperties::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mMass);
inStream.Read(mInertia);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,58 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
JPH_NAMESPACE_BEGIN
class StreamIn;
class StreamOut;
/// Describes the mass and inertia properties of a body. Used during body construction only.
class JPH_EXPORT MassProperties
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties)
public:
/// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$.
/// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes'
/// @param outRotation The rotation matrix R
/// @param outDiagonal The diagonal of the diagonal matrix D
/// @return True if successful, false if failed
bool DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const;
/// Set the mass and inertia of a box with edge size inBoxSize and density inDensity
void SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity);
/// Set the mass and scale the inertia tensor to match the mass
void ScaleToMass(float inMass);
/// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal
static Vec3 sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal);
/// Rotate the inertia by 3x3 matrix inRotation
void Rotate(Mat44Arg inRotation);
/// Translate the inertia by a vector inTranslation
void Translate(Vec3Arg inTranslation);
/// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape
void Scale(Vec3Arg inScale);
/// Saves the state of this object in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restore the state of this object from inStream.
void RestoreBinaryState(StreamIn &inStream);
/// Mass of the shape (kg)
float mMass = 0.0f;
/// Inertia tensor of the shape (kg m^2)
Mat44 mInertia = Mat44::sZero();
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,92 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Body/MotionProperties.h>
#include <Jolt/Physics/StateRecorder.h>
JPH_NAMESPACE_BEGIN
void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties)
{
// Store allowed DOFs
mAllowedDOFs = inAllowedDOFs;
// Decompose DOFs
uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111;
uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111;
// Set inverse mass
if (allowed_translation_axis == 0)
{
// No translation possible
mInvMass = 0.0f;
}
else
{
JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. "
"Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, "
"in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride.");
mInvMass = 1.0f / inMassProperties.mMass;
}
if (allowed_rotation_axis == 0)
{
// No rotation possible
mInvInertiaDiagonal = Vec3::sZero();
mInertiaRotation = Quat::sIdentity();
}
else
{
// Set inverse inertia
Mat44 rotation;
Vec3 diagonal;
if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal)
&& !diagonal.IsNearZero())
{
mInvInertiaDiagonal = diagonal.Reciprocal();
mInertiaRotation = rotation.GetQuaternion();
}
else
{
// Failed! Fall back to inertia tensor of sphere with radius 1.
mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
mInertiaRotation = Quat::sIdentity();
}
}
JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!");
}
void MotionProperties::SaveState(StateRecorder &inStream) const
{
// Only write properties that can change at runtime
inStream.Write(mLinearVelocity);
inStream.Write(mAngularVelocity);
inStream.Write(mForce);
inStream.Write(mTorque);
#ifdef JPH_DOUBLE_PRECISION
inStream.Write(mSleepTestOffset);
#endif // JPH_DOUBLE_PRECISION
inStream.Write(mSleepTestSpheres);
inStream.Write(mSleepTestTimer);
inStream.Write(mAllowSleeping);
}
void MotionProperties::RestoreState(StateRecorder &inStream)
{
inStream.Read(mLinearVelocity);
inStream.Read(mAngularVelocity);
inStream.Read(mForce);
inStream.Read(mTorque);
#ifdef JPH_DOUBLE_PRECISION
inStream.Read(mSleepTestOffset);
#endif // JPH_DOUBLE_PRECISION
inStream.Read(mSleepTestSpheres);
inStream.Read(mSleepTestTimer);
inStream.Read(mAllowSleeping);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,282 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Geometry/Sphere.h>
#include <Jolt/Physics/Body/AllowedDOFs.h>
#include <Jolt/Physics/Body/MotionQuality.h>
#include <Jolt/Physics/Body/BodyAccess.h>
#include <Jolt/Physics/Body/MotionType.h>
#include <Jolt/Physics/Body/BodyType.h>
#include <Jolt/Physics/Body/MassProperties.h>
#include <Jolt/Physics/DeterminismLog.h>
JPH_NAMESPACE_BEGIN
class StateRecorder;
/// Enum that determines if an object can go to sleep
enum class ECanSleep
{
CannotSleep = 0, ///< Object cannot go to sleep
CanSleep = 1, ///< Object can go to sleep
};
/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
class JPH_EXPORT MotionProperties
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Motion quality, or how well it detects collisions when it has a high velocity
EMotionQuality GetMotionQuality() const { return mMotionQuality; }
/// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties)
inline EAllowedDOFs GetAllowedDOFs() const { return mAllowedDOFs; }
/// If this body can go to sleep.
inline bool GetAllowSleeping() const { return mAllowSleeping; }
/// Get world space linear velocity of the center of mass
inline Vec3 GetLinearVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; }
/// Set world space linear velocity of the center of mass
void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); }
/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity
void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); }
/// Get world space angular velocity of the center of mass
inline Vec3 GetAngularVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; }
/// Set world space angular velocity of the center of mass
void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
/// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
inline void MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
///@name Velocity limits
///@{
/// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
inline float GetMaxLinearVelocity() const { return mMaxLinearVelocity; }
inline void SetMaxLinearVelocity(float inLinearVelocity) { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
/// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
inline float GetMaxAngularVelocity() const { return mMaxAngularVelocity; }
inline void SetMaxAngularVelocity(float inAngularVelocity) { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
///@}
/// Clamp velocity according to limit
inline void ClampLinearVelocity();
inline void ClampAngularVelocity();
/// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
inline float GetLinearDamping() const { return mLinearDamping; }
void SetLinearDamping(float inLinearDamping) { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
/// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
inline float GetAngularDamping() const { return mAngularDamping; }
void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
/// Get gravity factor (1 = normal gravity, 0 = no gravity)
inline float GetGravityFactor() const { return mGravityFactor; }
void SetGravityFactor(float inGravityFactor) { mGravityFactor = inGravityFactor; }
/// Set the mass and inertia tensor
void SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
/// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
inline float GetInverseMass() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
inline float GetInverseMassUnchecked() const { return mInvMass; }
/// Set the inverse mass (1 / mass).
/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
/// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
void SetInverseMass(float inInverseMass) { mInvMass = inInverseMass; }
/// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
inline Vec3 GetInverseInertiaDiagonal() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
/// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$
inline Quat GetInertiaRotation() const { return mInertiaRotation; }
/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
/// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
void SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot) { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
/// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass.
/// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted).
void ScaleToMass(float inMass);
/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
inline Mat44 GetLocalSpaceInverseInertia() const;
/// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic
inline Mat44 GetLocalSpaceInverseInertiaUnchecked() const;
/// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic.
inline Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const;
/// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic.
JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
JPH_INLINE Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
// Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
JPH_INLINE Vec3 GetAccumulatedForce() const { return Vec3::sLoadFloat3Unsafe(mForce); }
// Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
JPH_INLINE Vec3 GetAccumulatedTorque() const { return Vec3::sLoadFloat3Unsafe(mTorque); }
// Reset the total accumulated force, note that this will be done automatically after every time step.
JPH_INLINE void ResetForce() { mForce = Float3(0, 0, 0); }
// Reset the total accumulated torque, note that this will be done automatically after every time step.
JPH_INLINE void ResetTorque() { mTorque = Float3(0, 0, 0); }
// Reset the current velocity and accumulated force and torque.
JPH_INLINE void ResetMotion()
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
mLinearVelocity = mAngularVelocity = Vec3::sZero();
mForce = mTorque = Float3(0, 0, 0);
}
/// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
JPH_INLINE UVec4 GetLinearDOFsMask() const
{
UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
}
/// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
JPH_INLINE Vec3 LockTranslation(Vec3Arg inV) const
{
return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
}
/// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
JPH_INLINE UVec4 GetAngularDOFsMask() const
{
UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
}
/// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
JPH_INLINE Vec3 LockAngular(Vec3Arg inV) const
{
return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
}
/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; }
/// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; }
////////////////////////////////////////////////////////////
// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
////////////////////////////////////////////////////////////
///@name Update linear and angular velocity (used during constraint solving)
///@{
inline void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
inline void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
inline void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
inline void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
///@}
/// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
inline void ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
inline void ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
/// Access to the island index
uint32 GetIslandIndexInternal() const { return mIslandIndex; }
void SetIslandIndexInternal(uint32 inIndex) { mIslandIndex = inIndex; }
/// Access to the index in the active bodies array
uint32 GetIndexInActiveBodiesInternal() const { return mIndexInActiveBodies; }
#ifdef JPH_DOUBLE_PRECISION
inline DVec3 GetSleepTestOffset() const { return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
#endif // JPH_DOUBLE_PRECISION
/// Reset spheres to center around inPoints with radius 0
inline void ResetSleepTestSpheres(const RVec3 *inPoints);
/// Reset the sleep test timer without resetting the sleep test spheres
inline void ResetSleepTestTimer() { mSleepTestTimer = 0.0f; }
/// Accumulate sleep time and return if a body can go to sleep
inline ECanSleep AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
/// Saving state for replay
void SaveState(StateRecorder &inStream) const;
/// Restoring state for replay
void RestoreState(StateRecorder &inStream);
static constexpr uint32 cInactiveIndex = uint32(-1); ///< Constant indicating that body is not active
private:
friend class BodyManager;
friend class Body;
// 1st cache line
// 16 byte aligned
Vec3 mLinearVelocity { Vec3::sZero() }; ///< World space linear velocity of the center of mass (m/s)
Vec3 mAngularVelocity { Vec3::sZero() }; ///< World space angular velocity (rad/s)
Vec3 mInvInertiaDiagonal; ///< Diagonal of inverse inertia matrix: D
Quat mInertiaRotation; ///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1
// 2nd cache line
// 4 byte aligned
Float3 mForce { 0, 0, 0 }; ///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
Float3 mTorque { 0, 0, 0 }; ///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
float mInvMass; ///< Inverse mass of the object (1/kg)
float mLinearDamping; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
float mAngularDamping; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
float mMaxLinearVelocity; ///< Maximum linear velocity that this body can reach (m/s)
float mMaxAngularVelocity; ///< Maximum angular velocity that this body can reach (rad/s)
float mGravityFactor; ///< Factor to multiply gravity with
uint32 mIndexInActiveBodies = cInactiveIndex; ///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies)
uint32 mIslandIndex = cInactiveIndex; ///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
// 1 byte aligned
EMotionQuality mMotionQuality; ///< Motion quality, or how well it detects collisions when it has a high velocity
bool mAllowSleeping; ///< If this body can go to sleep
EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Allowed degrees of freedom for this body
uint8 mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
uint8 mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
// 3rd cache line (least frequently used)
// 4 byte aligned (or 8 byte if running in double precision)
#ifdef JPH_DOUBLE_PRECISION
Double3 mSleepTestOffset; ///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes.
#endif // JPH_DOUBLE_PRECISION
Sphere mSleepTestSpheres[3]; ///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
float mSleepTestTimer; ///< How long this body has been within the movement tolerance
#ifdef JPH_ENABLE_ASSERTS
EBodyType mCachedBodyType; ///< Copied from Body::mBodyType and cached for asserting purposes
EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes
#endif
};
JPH_NAMESPACE_END
#include "MotionProperties.inl"

View File

@@ -0,0 +1,178 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
JPH_ASSERT(mCachedMotionType != EMotionType::Static);
// Calculate required linear velocity
mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);
// Calculate required angular velocity
Vec3 axis;
float angle;
inDeltaRotation.GetAxisAngle(axis, angle);
mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));
}
void MotionProperties::ClampLinearVelocity()
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
float len_sq = mLinearVelocity.LengthSq();
JPH_ASSERT(isfinite(len_sq));
if (len_sq > Square(mMaxLinearVelocity))
mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq);
}
void MotionProperties::ClampAngularVelocity()
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
float len_sq = mAngularVelocity.LengthSq();
JPH_ASSERT(isfinite(len_sq));
if (len_sq > Square(mMaxAngularVelocity))
mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq);
}
inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
{
Mat44 rotation = Mat44::sRotation(mInertiaRotation);
Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
}
inline void MotionProperties::ScaleToMass(float inMass)
{
JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass");
JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero");
float new_inv_mass = 1.0f / inMass;
mInvInertiaDiagonal *= new_inv_mass / mInvMass;
mInvMass = new_inv_mass;
}
inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
{
JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
return GetLocalSpaceInverseInertiaUnchecked();
}
Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
{
JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation));
Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
// We need to mask out both the rows and columns of DOFs that are not allowed
Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat();
inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX())));
inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY())));
inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ())));
return inverse_inertia;
}
Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
{
JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
// Mask out columns of DOFs that are not allowed
Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat());
Vec3 v = Vec3::sAnd(inV, angular_dofs_mask);
// Multiply vector by inverse inertia
Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v));
// Mask out rows of DOFs that are not allowed
return Vec3::sAnd(result, angular_dofs_mask);
}
void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
// Calculate local space inertia tensor (a diagonal in local space)
UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sOne(), is_zero);
Vec3 nominator = Vec3::sSelect(Vec3::sOne(), Vec3::sZero(), is_zero);
Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
// Calculate local space angular momentum
Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation;
Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity;
Vec3 local_momentum = local_inertia * local_angular_velocity;
// The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor
// Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one
// to avoid introducing energy into the system due to the Euler step
Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum);
float new_local_momentum_len_sq = new_local_momentum.LengthSq();
new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero();
// Convert back to world space angular velocity
mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum);
}
void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
{
JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
// Update linear velocity
mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce()));
// Update angular velocity
mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());
// Linear damping: dv/dt = -c * v
// Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt)
// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime);
mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
// Clamp velocities
ClampLinearVelocity();
ClampAngularVelocity();
}
void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
{
#ifdef JPH_DOUBLE_PRECISION
// Make spheres relative to the first point and initialize them to zero radius
DVec3 offset = inPoints[0];
offset.StoreDouble3(&mSleepTestOffset);
mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f);
for (int i = 1; i < 3; ++i)
mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f);
#else
// Initialize the spheres to zero radius around the supplied points
for (int i = 0; i < 3; ++i)
mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
#endif
mSleepTestTimer = 0.0f;
}
ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep)
{
mSleepTestTimer += inDeltaTime;
return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,31 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
/// Motion quality, or how well it detects collisions when it has a high velocity
enum class EMotionQuality : uint8
{
/// Update the body in discrete steps. Body will tunnel through thin objects if its velocity is high enough.
/// This is the cheapest way of simulating a body.
Discrete,
/// Update the body using linear casting. When stepping the body, its collision shape is cast from
/// start to destination using the starting rotation. The body will not be able to tunnel through thin
/// objects at high velocity, but tunneling is still possible if the body is long and thin and has high
/// angular velocity. Time is stolen from the object (which means it will move up to the first collision
/// and will not bounce off the surface until the next integration step). This will make the body appear
/// to go slower when it collides with high velocity. In order to not get stuck, the body is always
/// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry.
///
/// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts
/// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1
/// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact
/// point added callback between A and B (which will be removed the next frame).
LinearCast,
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,17 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
JPH_NAMESPACE_BEGIN
/// Motion type of a physics body
enum class EMotionType : uint8
{
Static, ///< Non movable
Kinematic, ///< Movable using velocities only, does not respond to forces
Dynamic, ///< Responds to forces as a normal physics object
};
JPH_NAMESPACE_END