sid/NazaraEngine
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Physics3D: Switch from Newton Dynamics to Bullet3

Browse Source
This commit is contained in:
SirLynix
2023-03-07 19:17:49 +01:00
committed by Jérôme Leclercq
parent ec1efb5e56
commit 795efae3a0
13 changed files with 625 additions and 935 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/Nazara/Physics3D/BulletHelper.hpp Normal file
View File

@@ -0,0 +1,32 @@
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
// This file is part of the "Nazara Engine - Physics3D module"
// For conditions of distribution and use, see copyright notice in Config.hpp
#pragma once
#ifndef NAZARA_PHYSICS3D_BULLETHELPER_HPP
#define NAZARA_PHYSICS3D_BULLETHELPER_HPP
#include <Nazara/Prerequisites.hpp>
#include <Nazara/Math/Matrix4.hpp>
#include <Nazara/Math/Quaternion.hpp>
#include <Nazara/Math/Vector3.hpp>
#include <Nazara/Physics3D/Config.hpp>
#include <LinearMath/btQuaternion.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btVector3.h>
namespace Nz
{
inline Quaternionf FromBullet(const btQuaternion& quat);
inline Matrix4f FromBullet(const btTransform& transform);
inline Vector3f FromBullet(const btVector3& vec);
inline btTransform ToBullet(const Matrix4f& transformMatrix);
inline btQuaternion ToBullet(const Quaternionf& rotation);
inline btVector3 ToBullet(const Vector3f& vec);
}
#include <Nazara/Physics3D/BulletHelper.inl>
#endif // NAZARA_PHYSICS3D_BULLETHELPER_HPP

61
src/Nazara/Physics3D/BulletHelper.inl Normal file
View File

@@ -0,0 +1,61 @@
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
// This file is part of the "Nazara Engine - Physics3D module"
// For conditions of distribution and use, see copyright notice in Config.hpp
#include <Nazara/Physics3D/BulletHelper.hpp>
#include <Nazara/Physics3D/Debug.hpp>
namespace Nz
{
Quaternionf FromBullet(const btQuaternion& quat)
{
return Quaternionf(quat.w(), quat.x(), quat.y(), quat.z());
}
Matrix4f FromBullet(const btTransform& transform)
{
const btMatrix3x3& basisMatrix = transform.getBasis();
const btVector3& origin = transform.getOrigin();
const btVector3& row1 = basisMatrix.getRow(0);
const btVector3& row2 = basisMatrix.getRow(1);
const btVector3& row3 = basisMatrix.getRow(2);
return Matrix4f(
row1.x(), row1.y(), row1.z(), 0.f,
row2.x(), row2.y(), row2.z(), 0.f,
row3.x(), row3.y(), row3.z(), 0.f,
origin.x(), origin.y(), origin.z(), 1.f);
}
inline Vector3f FromBullet(const btVector3& vec)
{
return Vector3f(vec.x(), vec.y(), vec.z());
}
btTransform ToBullet(const Matrix4f& transformMatrix)
{
btTransform transform;
transform.setBasis(btMatrix3x3(
transformMatrix.m11, transformMatrix.m12, transformMatrix.m13,
transformMatrix.m21, transformMatrix.m22, transformMatrix.m23,
transformMatrix.m31, transformMatrix.m32, transformMatrix.m33
));
transform.setOrigin(btVector3(transformMatrix.m41, transformMatrix.m42, transformMatrix.m43));
return transform;
}
btQuaternion ToBullet(const Quaternionf& rotation)
{
return btQuaternion(rotation.x, rotation.y, rotation.z, rotation.w);
}
inline btVector3 ToBullet(const Vector3f& vec)
{
return btVector3(vec.x, vec.y, vec.z);
}
}
#include <Nazara/Physics3D/DebugOff.hpp>

485
src/Nazara/Physics3D/Collider3D.cpp
View File

@@ -4,46 +4,26 @@
#include <Nazara/Physics3D/Collider3D.hpp>
#include <Nazara/Core/PrimitiveList.hpp>
#include <Nazara/Physics3D/PhysWorld3D.hpp>
#include <Nazara/Physics3D/BulletHelper.hpp>
#include <Nazara/Utility/Algorithm.hpp>
#include <Nazara/Utility/IndexBuffer.hpp>
#include <Nazara/Utility/SoftwareBuffer.hpp>
#include <Nazara/Utility/StaticMesh.hpp>
#include <Nazara/Utility/VertexBuffer.hpp>
#include <newton/Newton.h>
#include <Nazara/Utils/MemoryHelper.hpp>
#include <tsl/ordered_map.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>
#include <BulletCollision/CollisionShapes/btCapsuleShape.h>
#include <BulletCollision/CollisionShapes/btCompoundShape.h>
#include <BulletCollision/CollisionShapes/btConeShape.h>
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
#include <BulletCollision/CollisionShapes/btCylinderShape.h>
#include <BulletCollision/CollisionShapes/btSphereShape.h>
#include <Nazara/Physics3D/Debug.hpp>
namespace Nz
{
namespace
{
std::shared_ptr<Collider3D> CreateGeomFromPrimitive(const Primitive& primitive)
{
switch (primitive.type)
{
case PrimitiveType::Box:
return std::make_shared<BoxCollider3D>(primitive.box.lengths, primitive.matrix);
case PrimitiveType::Cone:
return std::make_shared<ConeCollider3D>(primitive.cone.length, primitive.cone.radius, primitive.matrix);
case PrimitiveType::Plane:
return std::make_shared<BoxCollider3D>(Vector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y), primitive.matrix);
///TODO: PlaneGeom?
case PrimitiveType::Sphere:
return std::make_shared<SphereCollider3D>(primitive.sphere.size, primitive.matrix.GetTranslation());
}
NazaraError("Primitive type not handled (0x" + NumberToString(UnderlyingCast(primitive.type), 16) + ')');
return std::shared_ptr<Collider3D>();
}
}
Collider3D::~Collider3D()
{
for (auto& pair : m_handles)
NewtonDestroyCollision(pair.second);
}
Collider3D::~Collider3D() = default;
Boxf Collider3D::ComputeAABB(const Vector3f& translation, const Quaternionf& rotation, const Vector3f& scale) const
{
@@ -52,122 +32,29 @@ namespace Nz
Boxf Collider3D::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) const
{
Vector3f min, max;
btTransform transform = ToBullet(offsetMatrix);
// Check for existing collision handles, and create a temporary one if none is available
if (m_handles.empty())
{
PhysWorld3D world;
btVector3 min, max;
GetShape()->getAabb(transform, min, max);
NewtonCollision* collision = CreateHandle(&world);
{
NewtonCollisionCalculateAABB(collision, &offsetMatrix.m11, &min.x, &max.x);
}
NewtonDestroyCollision(collision);
}
else
NewtonCollisionCalculateAABB(m_handles.begin()->second, &offsetMatrix.m11, &min.x, &max.x);
return Boxf(scale * min, scale * max);
return Boxf(scale * FromBullet(min), scale * FromBullet(max));
}
void Collider3D::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
void Collider3D::ComputeInertia(float mass, Vector3f* inertia) const
{
float inertiaMatrix[3];
float origin[3];
NazaraAssert(inertia, "invalid inertia pointer");
// Check for existing collision handles, and create a temporary one if none is available
if (m_handles.empty())
{
PhysWorld3D world;
btVector3 inertiaVec;
GetShape()->calculateLocalInertia(mass, inertiaVec);
NewtonCollision* collision = CreateHandle(&world);
{
NewtonConvexCollisionCalculateInertialMatrix(collision, inertiaMatrix, origin);
}
NewtonDestroyCollision(collision);
}
else
NewtonConvexCollisionCalculateInertialMatrix(m_handles.begin()->second, inertiaMatrix, origin);
if (inertia)
inertia->Set(inertiaMatrix);
if (center)
center->Set(origin);
*inertia = FromBullet(inertiaVec);
}
float Collider3D::ComputeVolume() const
{
float volume;
// Check for existing collision handles, and create a temporary one if none is available
if (m_handles.empty())
{
PhysWorld3D world;
NewtonCollision* collision = CreateHandle(&world);
{
volume = NewtonConvexCollisionCalculateVolume(collision);
}
NewtonDestroyCollision(collision);
}
else
volume = NewtonConvexCollisionCalculateVolume(m_handles.begin()->second);
return volume;
}
void Collider3D::ForEachPolygon(const std::function<void(const Vector3f* vertices, std::size_t vertexCount)>& callback) const
{
auto newtCallback = [](void* const userData, int vertexCount, const dFloat* const faceArray, int /*faceId*/)
{
static_assert(sizeof(Vector3f) == 3 * sizeof(float), "Vector3 is expected to contain 3 floats without padding");
const auto& cb = *static_cast<std::add_pointer_t<decltype(callback)>>(userData);
cb(reinterpret_cast<const Vector3f*>(faceArray), vertexCount);
};
// Check for existing collision handles, and create a temporary one if none is available
Matrix4f identity = Matrix4f::Identity();
if (m_handles.empty())
{
PhysWorld3D world;
NewtonCollision* collision = CreateHandle(&world);
{
NewtonCollisionForEachPolygonDo(collision, &identity.m11, newtCallback, const_cast<void*>(static_cast<const void*>(&callback))); //< This isn't that bad; pointer will not be used for writing
}
NewtonDestroyCollision(collision);
}
else
NewtonCollisionForEachPolygonDo(m_handles.begin()->second, &identity.m11, newtCallback, const_cast<void*>(static_cast<const void*>(&callback))); //< This isn't that bad; pointer will not be used for writing
}
std::shared_ptr<StaticMesh> Collider3D::GenerateMesh() const
std::shared_ptr<StaticMesh> Collider3D::GenerateDebugMesh() const
{
std::vector<Vector3f> colliderVertices;
std::vector<UInt16> colliderIndices;
// Generate a line list
ForEachPolygon([&](const Vector3f* vertices, std::size_t vertexCount)
{
UInt16 firstIndex = SafeCast<UInt16>(colliderVertices.size());
for (std::size_t i = 0; i < vertexCount; ++i)
colliderVertices.push_back(vertices[i]);
for (std::size_t i = 1; i < vertexCount; ++i)
{
colliderIndices.push_back(SafeCast<UInt16>(firstIndex + i - 1));
colliderIndices.push_back(SafeCast<UInt16>(firstIndex + i));
}
if (vertexCount > 2)
{
colliderIndices.push_back(SafeCast<UInt16>(firstIndex + vertexCount - 1));
colliderIndices.push_back(SafeCast<UInt16>(firstIndex));
}
});
BuildDebugMesh(colliderVertices, colliderIndices);
std::shared_ptr<VertexBuffer> colliderVB = std::make_shared<VertexBuffer>(VertexDeclaration::Get(VertexLayout::XYZ), SafeCast<UInt32>(colliderVertices.size()), BufferUsage::Write, SoftwareBufferFactory, colliderVertices.data());
std::shared_ptr<IndexBuffer> colliderIB = std::make_shared<IndexBuffer>(IndexType::U16, colliderIndices.size(), BufferUsage::Write, SoftwareBufferFactory, colliderIndices.data());
@@ -179,60 +66,105 @@ namespace Nz
return colliderSubMesh;
}
NewtonCollision* Collider3D::GetHandle(PhysWorld3D* world) const
{
auto it = m_handles.find(world);
if (it == m_handles.end())
it = m_handles.insert(std::make_pair(world, CreateHandle(world))).first;
return it->second;
}
std::shared_ptr<Collider3D> Collider3D::Build(const PrimitiveList& list)
{
std::size_t primitiveCount = list.GetSize();
if (primitiveCount > 1)
{
std::vector<std::shared_ptr<Collider3D>> geoms(primitiveCount);
std::vector<CompoundCollider3D::ChildCollider> childColliders(primitiveCount);
for (unsigned int i = 0; i < primitiveCount; ++i)
geoms[i] = CreateGeomFromPrimitive(list.GetPrimitive(i));
{
const Primitive& primitive = list.GetPrimitive(i);
childColliders[i].collider = CreateGeomFromPrimitive(primitive);
childColliders[i].offsetMatrix = primitive.matrix;
}
return std::make_shared<CompoundCollider3D>(std::move(geoms));
return std::make_shared<CompoundCollider3D>(std::move(childColliders));
}
else if (primitiveCount > 0)
return CreateGeomFromPrimitive(list.GetPrimitive(0));
else
return std::make_shared<NullCollider3D>();
}
std::shared_ptr<Collider3D> Collider3D::CreateGeomFromPrimitive(const Primitive& primitive)
{
switch (primitive.type)
{
case PrimitiveType::Box:
return std::make_shared<BoxCollider3D>(primitive.box.lengths);
case PrimitiveType::Cone:
return std::make_shared<ConeCollider3D>(primitive.cone.length, primitive.cone.radius);
case PrimitiveType::Plane:
return std::make_shared<BoxCollider3D>(Vector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y));
///TODO: PlaneGeom?
case PrimitiveType::Sphere:
return std::make_shared<SphereCollider3D>(primitive.sphere.size);
}
NazaraError("Primitive type not handled (0x" + NumberToString(UnderlyingCast(primitive.type), 16) + ')');
return std::shared_ptr<Collider3D>();
}
/********************************** BoxCollider3D **********************************/
BoxCollider3D::BoxCollider3D(const Vector3f& lengths, const Matrix4f& transformMatrix) :
m_matrix(transformMatrix),
BoxCollider3D::BoxCollider3D(const Vector3f& lengths) :
m_lengths(lengths)
{
m_shape = std::make_unique<btBoxShape>(ToBullet(m_lengths));
}
BoxCollider3D::BoxCollider3D(const Vector3f& lengths, const Vector3f& translation, const Quaternionf& rotation) :
BoxCollider3D(lengths, Matrix4f::Transform(translation, rotation))
BoxCollider3D::~BoxCollider3D() = default;
void BoxCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
auto InsertVertex = [&](float x, float y, float z) -> UInt16
{
UInt16 index = SafeCast<UInt16>(vertices.size());
vertices.push_back(offsetMatrix * Vector3f(x, y, z));
return index;
};
Vector3f halfLengths = m_lengths * 0.5f;
UInt16 v0 = InsertVertex(-halfLengths.x, -halfLengths.y, -halfLengths.z);
UInt16 v1 = InsertVertex(halfLengths.x, -halfLengths.y, -halfLengths.z);
UInt16 v2 = InsertVertex(halfLengths.x, -halfLengths.y, halfLengths.z);
UInt16 v3 = InsertVertex(-halfLengths.x, -halfLengths.y, halfLengths.z);
UInt16 v4 = InsertVertex(-halfLengths.x, halfLengths.y, -halfLengths.z);
UInt16 v5 = InsertVertex(halfLengths.x, halfLengths.y, -halfLengths.z);
UInt16 v6 = InsertVertex(halfLengths.x, halfLengths.y, halfLengths.z);
UInt16 v7 = InsertVertex(-halfLengths.x, halfLengths.y, halfLengths.z);
auto InsertEdge = [&](UInt16 from, UInt16 to)
{
indices.push_back(from);
indices.push_back(to);
};
InsertEdge(v0, v1);
InsertEdge(v1, v2);
InsertEdge(v2, v3);
InsertEdge(v3, v0);
InsertEdge(v4, v5);
InsertEdge(v5, v6);
InsertEdge(v6, v7);
InsertEdge(v7, v4);
InsertEdge(v0, v4);
InsertEdge(v1, v5);
InsertEdge(v2, v6);
InsertEdge(v3, v7);
}
Boxf BoxCollider3D::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) const
btCollisionShape* BoxCollider3D::GetShape() const
{
Vector3f halfLengths(m_lengths * 0.5f);
Boxf aabb(-halfLengths.x, -halfLengths.y, -halfLengths.z, m_lengths.x, m_lengths.y, m_lengths.z);
aabb.Transform(offsetMatrix, true);
aabb.Scale(scale);
return aabb;
}
float BoxCollider3D::ComputeVolume() const
{
return m_lengths.x * m_lengths.y * m_lengths.z;
return m_shape.get();
}
Vector3f BoxCollider3D::GetLengths() const
@@ -245,22 +177,18 @@ namespace Nz
return ColliderType3D::Box;
}
NewtonCollision* BoxCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateBox(world->GetHandle(), m_lengths.x, m_lengths.y, m_lengths.z, 0, &m_matrix.m11);
}
/******************************** CapsuleCollider3D ********************************/
CapsuleCollider3D::CapsuleCollider3D(float length, float radius, const Matrix4f& transformMatrix) :
m_matrix(transformMatrix),
CapsuleCollider3D::CapsuleCollider3D(float length, float radius) :
m_length(length),
m_radius(radius)
{
m_shape = std::make_unique<btCapsuleShape>(radius, length);
}
CapsuleCollider3D::CapsuleCollider3D(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
CapsuleCollider3D(length, radius, Matrix4f::Transform(translation, rotation))
CapsuleCollider3D::~CapsuleCollider3D() = default;
void CapsuleCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
}
@@ -274,26 +202,45 @@ namespace Nz
return m_radius;
}
btCollisionShape* CapsuleCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D CapsuleCollider3D::GetType() const
{
return ColliderType3D::Capsule;
}
NewtonCollision* CapsuleCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateCapsule(world->GetHandle(), m_radius, m_radius, m_length, 0, &m_matrix.m11);
}
/******************************* CompoundCollider3D ********************************/
CompoundCollider3D::CompoundCollider3D(std::vector<std::shared_ptr<Collider3D>> geoms) :
m_geoms(std::move(geoms))
CompoundCollider3D::CompoundCollider3D(std::vector<ChildCollider> childs) :
m_childs(std::move(childs))
{
m_shape = std::make_unique<btCompoundShape>();
for (const auto& child : m_childs)
{
btTransform transform = ToBullet(child.offsetMatrix);
m_shape->addChildShape(transform, child.collider->GetShape());
}
}
const std::vector<std::shared_ptr<Collider3D>>& CompoundCollider3D::GetGeoms() const
CompoundCollider3D::~CompoundCollider3D() = default;
void CompoundCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
return m_geoms;
for (const auto& child : m_childs)
child.collider->BuildDebugMesh(vertices, indices, offsetMatrix * child.offsetMatrix);
}
auto CompoundCollider3D::GetGeoms() const -> const std::vector<ChildCollider>&
{
return m_childs;
}
btCollisionShape* CompoundCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D CompoundCollider3D::GetType() const
@@ -301,38 +248,18 @@ namespace Nz
return ColliderType3D::Compound;
}
NewtonCollision* CompoundCollider3D::CreateHandle(PhysWorld3D* world) const
{
NewtonCollision* compoundCollision = NewtonCreateCompoundCollision(world->GetHandle(), 0);
NewtonCompoundCollisionBeginAddRemove(compoundCollision);
for (const std::shared_ptr<Collider3D>& geom : m_geoms)
{
if (geom->GetType() == ColliderType3D::Compound)
{
CompoundCollider3D& compoundGeom = static_cast<CompoundCollider3D&>(*geom);
for (const std::shared_ptr<Collider3D>& piece : compoundGeom.GetGeoms())
NewtonCompoundCollisionAddSubCollision(compoundCollision, piece->GetHandle(world));
}
else
NewtonCompoundCollisionAddSubCollision(compoundCollision, geom->GetHandle(world));
}
NewtonCompoundCollisionEndAddRemove(compoundCollision);
return compoundCollision;
}
/********************************* ConeCollider3D **********************************/
ConeCollider3D::ConeCollider3D(float length, float radius, const Matrix4f& transformMatrix) :
m_matrix(transformMatrix),
ConeCollider3D::ConeCollider3D(float length, float radius) :
m_length(length),
m_radius(radius)
{
m_shape = std::make_unique<btConeShape>(radius, length);
}
ConeCollider3D::ConeCollider3D(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
ConeCollider3D(length, radius, Matrix4f::Transform(translation, rotation))
ConeCollider3D::~ConeCollider3D() = default;
void ConeCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
}
@@ -346,35 +273,59 @@ namespace Nz
return m_radius;
}
btCollisionShape* ConeCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D ConeCollider3D::GetType() const
{
return ColliderType3D::Cone;
}
NewtonCollision* ConeCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateCone(world->GetHandle(), m_radius, m_length, 0, &m_matrix.m11);
}
/****************************** ConvexCollider3D *******************************/
ConvexCollider3D::ConvexCollider3D(SparsePtr<const Vector3f> vertices, unsigned int vertexCount, float tolerance, const Matrix4f& transformMatrix) :
m_matrix(transformMatrix),
m_tolerance(tolerance)
ConvexCollider3D::ConvexCollider3D(SparsePtr<const Vector3f> vertices, unsigned int vertexCount)
{
m_vertices.resize(vertexCount);
if (vertices.GetStride() != sizeof(Vector3f))
{
for (unsigned int i = 0; i < vertexCount; ++i)
m_vertices[i] = *vertices++;
}
else // Fast path
std::memcpy(m_vertices.data(), vertices.GetPtr(), vertexCount*sizeof(Vector3f));
static_assert(std::is_same_v<btScalar, float>);
m_shape = std::make_unique<btConvexHullShape>(reinterpret_cast<const float*>(vertices.GetPtr()), vertexCount, vertices.GetStride());
m_shape->optimizeConvexHull();
}
ConvexCollider3D::ConvexCollider3D(SparsePtr<const Vector3f> vertices, unsigned int vertexCount, float tolerance, const Vector3f& translation, const Quaternionf& rotation) :
ConvexCollider3D(vertices, vertexCount, tolerance, Matrix4f::Transform(translation, rotation))
ConvexCollider3D::~ConvexCollider3D() = default;
void ConvexCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
tsl::ordered_map<Vector3f, UInt16> vertexCache;
auto InsertVertex = [&](const Vector3f& position) -> UInt16
{
auto it = vertexCache.find(position);
if (it != vertexCache.end())
return it->second;
UInt16 index = SafeCast<UInt16>(vertices.size());
vertices.push_back(position);
vertexCache.emplace(position, index);
return index;
};
int edgeCount = m_shape->getNumEdges();
for (int i = 0; i < edgeCount; ++i)
{
btVector3 from, to;
m_shape->getEdge(i, from, to);
indices.push_back(InsertVertex(offsetMatrix * FromBullet(from)));
indices.push_back(InsertVertex(offsetMatrix * FromBullet(to)));
}
}
btCollisionShape* ConvexCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D ConvexCollider3D::GetType() const
@@ -382,22 +333,19 @@ namespace Nz
return ColliderType3D::ConvexHull;
}
NewtonCollision* ConvexCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateConvexHull(world->GetHandle(), static_cast<int>(m_vertices.size()), reinterpret_cast<const float*>(m_vertices.data()), sizeof(Vector3f), m_tolerance, 0, &m_matrix.m11);
}
/******************************* CylinderCollider3D ********************************/
CylinderCollider3D::CylinderCollider3D(float length, float radius, const Matrix4f& transformMatrix) :
m_matrix(transformMatrix),
CylinderCollider3D::CylinderCollider3D(float length, float radius) :
m_length(length),
m_radius(radius)
{
// TODO: Allow to use two different radius
m_shape = std::make_unique<btCylinderShape>(btVector3{ radius, length, radius });
}
CylinderCollider3D::CylinderCollider3D(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
CylinderCollider3D(length, radius, Matrix4f::Transform(translation, rotation))
CylinderCollider3D::~CylinderCollider3D() = default;
void CylinderCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
}
@@ -411,65 +359,49 @@ namespace Nz
return m_radius;
}
btCollisionShape* CylinderCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D CylinderCollider3D::GetType() const
{
return ColliderType3D::Cylinder;
}
NewtonCollision* CylinderCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateCylinder(world->GetHandle(), m_radius, m_radius, m_length, 0, &m_matrix.m11);
}
/********************************* NullCollider3D **********************************/
NullCollider3D::NullCollider3D()
void NullCollider3D::BuildDebugMesh(std::vector<Vector3f>& /*vertices*/, std::vector<UInt16>& /*indices*/, const Matrix4f& /*offsetMatrix*/) const
{
}
void NullCollider3D::ComputeInertia(float /*mass*/, Vector3f* inertia) const
{
inertia->Set(1.f, 1.f, 1.f);
}
btCollisionShape* NullCollider3D::GetShape() const
{
return nullptr;
}
ColliderType3D NullCollider3D::GetType() const
{
return ColliderType3D::Null;
}
void NullCollider3D::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
{
if (inertia)
inertia->MakeUnit();
if (center)
center->MakeZero();
}
NewtonCollision* NullCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateNull(world->GetHandle());
}
/******************************** SphereCollider3D *********************************/
SphereCollider3D::SphereCollider3D(float radius, const Matrix4f& transformMatrix) :
SphereCollider3D(radius, transformMatrix.GetTranslation())
{
}
SphereCollider3D::SphereCollider3D(float radius, const Vector3f& translation, const Quaternionf& /*rotation*/) :
m_position(translation),
SphereCollider3D::SphereCollider3D(float radius) :
m_radius(radius)
{
m_shape = std::make_unique<btSphereShape>(radius);
}
Boxf SphereCollider3D::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) const
{
Vector3f size(m_radius * Sqrt5<float> * scale);
Vector3f position(offsetMatrix.GetTranslation());
SphereCollider3D::~SphereCollider3D() = default;
return Boxf(position - size, position + size);
}
float SphereCollider3D::ComputeVolume() const
void SphereCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
{
return Pi<float> * m_radius * m_radius * m_radius / 3.f;
}
float SphereCollider3D::GetRadius() const
@@ -477,14 +409,13 @@ namespace Nz
return m_radius;
}
btCollisionShape* SphereCollider3D::GetShape() const
{
return m_shape.get();
}
ColliderType3D SphereCollider3D::GetType() const
{
return ColliderType3D::Sphere;
}
NewtonCollision* SphereCollider3D::CreateHandle(PhysWorld3D* world) const
{
Matrix4f transformMatrix = Matrix4f::Translate(m_position);
return NewtonCreateSphere(world->GetHandle(), m_radius, 0, &transformMatrix.m11);
}
}

225
src/Nazara/Physics3D/PhysWorld3D.cpp
View File

@@ -3,82 +3,60 @@
// For conditions of distribution and use, see copyright notice in Config.hpp
#include <Nazara/Physics3D/PhysWorld3D.hpp>
#include <NazaraUtils/StackVector.hpp>
#include <newton/Newton.h>
#include <Nazara/Physics3D/BulletHelper.hpp>
#include <Nazara/Utils/StackVector.hpp>
#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
#include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
#include <cassert>
#include <Nazara/Physics3D/Debug.hpp>
namespace Nz
{
struct PhysWorld3D::BulletWorld
{
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher;
btDbvtBroadphase broadphase;
btSequentialImpulseConstraintSolver constraintSolver;
btDiscreteDynamicsWorld dynamicWorld;
BulletWorld() :
dispatcher(&collisionConfiguration),
dynamicWorld(&dispatcher, &broadphase, &constraintSolver, &collisionConfiguration)
{
}
BulletWorld(const BulletWorld&) = delete;
BulletWorld(BulletWorld&&) = delete;
BulletWorld& operator=(const BulletWorld&) = delete;
BulletWorld& operator=(BulletWorld&&) = delete;
};
PhysWorld3D::PhysWorld3D() :
m_maxStepCount(50),
m_gravity(Vector3f::Zero()),
m_stepSize(Time::TickDuration(120)),
m_timestepAccumulator(Time::Zero())
{
m_world = NewtonCreate();
NewtonWorldSetUserData(m_world, this);
m_materialIds.emplace("default", NewtonMaterialGetDefaultGroupID(m_world));
m_world = std::make_unique<BulletWorld>();
}
PhysWorld3D::PhysWorld3D(PhysWorld3D&& physWorld) noexcept :
m_callbacks(std::move(physWorld.m_callbacks)),
m_materialIds(std::move(physWorld.m_materialIds)),
m_maxStepCount(std::move(physWorld.m_maxStepCount)),
m_world(std::move(physWorld.m_world)),
m_gravity(std::move(physWorld.m_gravity)),
m_stepSize(std::move(physWorld.m_stepSize)),
m_timestepAccumulator(std::move(physWorld.m_timestepAccumulator))
PhysWorld3D::PhysWorld3D(PhysWorld3D&& physWorld) noexcept = default;
PhysWorld3D::~PhysWorld3D() = default;
btDynamicsWorld* PhysWorld3D::GetDynamicsWorld()
{
NewtonWorldSetUserData(m_world, this);
}
PhysWorld3D::~PhysWorld3D()
{
if (m_world)
NewtonDestroy(m_world);
}
int PhysWorld3D::CreateMaterial(std::string name)
{
NazaraAssert(m_materialIds.find(name) == m_materialIds.end(), "Material \"" + name + "\" already exists");
int materialId = NewtonMaterialCreateGroupID(m_world);
m_materialIds.emplace(std::move(name), materialId);
return materialId;
}
void PhysWorld3D::ForEachBodyInAABB(const Boxf& box, const BodyIterator& iterator)
{
auto NewtonCallback = [](const NewtonBody* const body, void* const userdata) -> int
{
const BodyIterator& bodyIterator = *static_cast<BodyIterator*>(userdata);
return bodyIterator(*static_cast<RigidBody3D*>(NewtonBodyGetUserData(body)));
};
Vector3f min = box.GetMinimum();
Vector3f max = box.GetMaximum();
NewtonWorldForEachBodyInAABBDo(m_world, &min.x, &max.x, NewtonCallback, const_cast<void*>(static_cast<const void*>(&iterator)));
return &m_world->dynamicWorld;
}
Vector3f PhysWorld3D::GetGravity() const
{
return m_gravity;
}
NewtonWorld* PhysWorld3D::GetHandle() const
{
return m_world;
}
int PhysWorld3D::GetMaterial(const std::string& name)
{
auto it = m_materialIds.find(name);
NazaraAssert(it != m_materialIds.end(), "Material \"" + name + "\" does not exists");
return it->second;
return FromBullet(m_world->dynamicWorld.getGravity());
}
std::size_t PhysWorld3D::GetMaxStepCount() const
@@ -91,14 +69,9 @@ namespace Nz
return m_stepSize;
}
unsigned int PhysWorld3D::GetThreadCount() const
{
return NewtonGetThreadsCount(m_world);
}
void PhysWorld3D::SetGravity(const Vector3f& gravity)
{
m_gravity = gravity;
m_world->dynamicWorld.setGravity(ToBullet(gravity));
}
void PhysWorld3D::SetMaxStepCount(std::size_t maxStepCount)
@@ -111,136 +84,20 @@ namespace Nz
m_stepSize = stepSize;
}
void PhysWorld3D::SetThreadCount(unsigned int threadCount)
{
NewtonSetThreadsCount(m_world, threadCount);
}
void PhysWorld3D::SetMaterialCollisionCallback(int firstMaterial, int secondMaterial, AABBOverlapCallback aabbOverlapCallback, CollisionCallback collisionCallback)
{
static_assert(sizeof(UInt64) >= 2 * sizeof(int), "Oops");
auto callbackPtr = std::make_unique<Callback>();
callbackPtr->aabbOverlapCallback = std::move(aabbOverlapCallback);
callbackPtr->collisionCallback = std::move(collisionCallback);
NewtonMaterialSetCollisionCallback(m_world, firstMaterial, secondMaterial, (callbackPtr->aabbOverlapCallback) ? OnAABBOverlap : nullptr, (callbackPtr->collisionCallback) ? ProcessContact : nullptr);
NewtonMaterialSetCallbackUserData(m_world, firstMaterial, secondMaterial, callbackPtr.get());
UInt64 firstMaterialId(firstMaterial);
UInt64 secondMaterialId(secondMaterial);
UInt64 callbackIndex = firstMaterialId << 32 | secondMaterialId;
m_callbacks[callbackIndex] = std::move(callbackPtr);
}
void PhysWorld3D::SetMaterialDefaultCollidable(int firstMaterial, int secondMaterial, bool collidable)
{
NewtonMaterialSetDefaultCollidable(m_world, firstMaterial, secondMaterial, collidable);
}
void PhysWorld3D::SetMaterialDefaultElasticity(int firstMaterial, int secondMaterial, float elasticCoef)
{
NewtonMaterialSetDefaultElasticity(m_world, firstMaterial, secondMaterial, elasticCoef);
}
void PhysWorld3D::SetMaterialDefaultFriction(int firstMaterial, int secondMaterial, float staticFriction, float kineticFriction)
{
NewtonMaterialSetDefaultFriction(m_world, firstMaterial, secondMaterial, staticFriction, kineticFriction);
}
void PhysWorld3D::SetMaterialDefaultSoftness(int firstMaterial, int secondMaterial, float softness)
{
NewtonMaterialSetDefaultSoftness(m_world, firstMaterial, secondMaterial, softness);
}
void PhysWorld3D::SetMaterialSurfaceThickness(int firstMaterial, int secondMaterial, float thickness)
{
NewtonMaterialSetSurfaceThickness(m_world, firstMaterial, secondMaterial, thickness);
}
void PhysWorld3D::Step(Time timestep)
{
m_timestepAccumulator += timestep;
std::size_t stepCount = 0;
float dt = m_stepSize.AsSeconds<float>();
btScalar stepSize = m_stepSize.AsSeconds<btScalar>();
std::size_t stepCount = 0;
while (m_timestepAccumulator >= m_stepSize && stepCount < m_maxStepCount)
{
NewtonUpdate(m_world, dt);
m_world->dynamicWorld.stepSimulation(stepSize, 0, stepSize);
m_timestepAccumulator -= m_stepSize;
stepCount++;
}
}
PhysWorld3D& PhysWorld3D::operator=(PhysWorld3D&& physWorld) noexcept
{
if (m_world)
NewtonDestroy(m_world);
m_callbacks = std::move(physWorld.m_callbacks);
m_materialIds = std::move(physWorld.m_materialIds);
m_maxStepCount = std::move(physWorld.m_maxStepCount);
m_world = std::move(physWorld.m_world);
m_gravity = std::move(physWorld.m_gravity);
m_stepSize = std::move(physWorld.m_stepSize);
m_timestepAccumulator = std::move(physWorld.m_timestepAccumulator);
NewtonWorldSetUserData(m_world, this);
return *this;
}
int PhysWorld3D::OnAABBOverlap(const NewtonJoint* const contactJoint, float /*timestep*/, int /*threadIndex*/)
{
RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint)));
RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint)));
assert(bodyA && bodyB);
using ContactJoint = void*;
// Query all joints first, to prevent removing a joint from the list while iterating on it
StackVector<ContactJoint> contacts = NazaraStackVector(ContactJoint, NewtonContactJointGetContactCount(contactJoint));
for (ContactJoint contact = NewtonContactJointGetFirstContact(contactJoint); contact; contact = NewtonContactJointGetNextContact(contactJoint, contact))
contacts.push_back(contact);
for (ContactJoint contact : contacts)
{
NewtonMaterial* material = NewtonContactGetMaterial(contact);
Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
assert(callbackData);
assert(callbackData->collisionCallback);
if (!callbackData->collisionCallback(*bodyA, *bodyB))
return 0;
}
return 1;
}
void PhysWorld3D::ProcessContact(const NewtonJoint* const contactJoint, float /*timestep*/, int /*threadIndex*/)
{
RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody0(contactJoint)));
RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(NewtonJointGetBody1(contactJoint)));
assert(bodyA && bodyB);
using ContactJoint = void*;
// Query all joints first, to prevent removing a joint from the list while iterating on it
StackVector<ContactJoint> contacts = NazaraStackVector(ContactJoint, NewtonContactJointGetContactCount(contactJoint));
for (ContactJoint contact = NewtonContactJointGetFirstContact(contactJoint); contact; contact = NewtonContactJointGetNextContact(contactJoint, contact))
contacts.push_back(contact);
for (ContactJoint contact : contacts)
{
NewtonMaterial* material = NewtonContactGetMaterial(contact);
Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
assert(callbackData);
assert(callbackData->collisionCallback);
if (!callbackData->collisionCallback(*bodyA, *bodyB))
NewtonContactJointRemoveContact(contactJoint, contact);
}
}
PhysWorld3D& PhysWorld3D::operator=(PhysWorld3D&& physWorld) noexcept = default;
}

6
src/Nazara/Physics3D/Physics3D.cpp
View File

@@ -8,7 +8,6 @@
#include <Nazara/Core/Log.hpp>
#include <Nazara/Physics3D/Collider3D.hpp>
#include <Nazara/Physics3D/Config.hpp>
#include <newton/Newton.h>
#include <Nazara/Physics3D/Debug.hpp>
namespace Nz
@@ -18,10 +17,5 @@ namespace Nz
{
}
unsigned int Physics3D::GetMemoryUsed()
{
return NewtonGetMemoryUsed();
}
Physics3D* Physics3D::s_instance;
}

357
src/Nazara/Physics3D/RigidBody3D.cpp
View File

@@ -3,8 +3,10 @@
// For conditions of distribution and use, see copyright notice in Config.hpp
#include <Nazara/Physics3D/RigidBody3D.hpp>
#include <Nazara/Physics3D/BulletHelper.hpp>
#include <Nazara/Physics3D/PhysWorld3D.hpp>
#include <newton/Newton.h>
#include <BulletDynamics/Dynamics/btDynamicsWorld.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>
#include <algorithm>
#include <array>
#include <cmath>
@@ -13,66 +15,30 @@
namespace Nz
{
RigidBody3D::RigidBody3D(PhysWorld3D* world, const Matrix4f& mat) :
RigidBody3D(world, std::make_shared<NullCollider3D>(), mat)
RigidBody3D(world, nullptr, mat)
{
}
RigidBody3D::RigidBody3D(PhysWorld3D* world, std::shared_ptr<Collider3D> geom, const Matrix4f& mat) :
m_geom(std::move(geom)),
m_forceAccumulator(Vector3f::Zero()),
m_torqueAccumulator(Vector3f::Zero()),
m_world(world),
m_gravityFactor(1.f),
m_mass(0.f)
m_world(world)
{
NazaraAssert(m_world, "Invalid world");
if (!m_geom)
m_geom = std::make_shared<NullCollider3D>();
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), &mat.m11);
NewtonBodySetUserData(m_body, this);
Vector3f inertia;
m_geom->ComputeInertia(1.f, &inertia);
btRigidBody::btRigidBodyConstructionInfo constructionInfo(1.f, nullptr, m_geom->GetShape(), ToBullet(inertia));
m_body = std::make_unique<btRigidBody>(constructionInfo);
m_world->GetDynamicsWorld()->addRigidBody(m_body.get());
}
RigidBody3D::RigidBody3D(const RigidBody3D& object) :
m_geom(object.m_geom),
m_forceAccumulator(Vector3f::Zero()),
m_torqueAccumulator(Vector3f::Zero()),
m_world(object.m_world),
m_gravityFactor(object.m_gravityFactor),
m_mass(0.f)
{
NazaraAssert(m_world, "Invalid world");
NazaraAssert(m_geom, "Invalid geometry");
std::array<float, 16> transformMatrix;
NewtonBodyGetMatrix(object.GetHandle(), transformMatrix.data());
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), transformMatrix.data());
NewtonBodySetUserData(m_body, this);
SetMass(object.m_mass);
SetAngularDamping(object.GetAngularDamping());
SetAngularVelocity(object.GetAngularVelocity());
SetLinearDamping(object.GetLinearDamping());
SetLinearVelocity(object.GetLinearVelocity());
SetMassCenter(object.GetMassCenter());
SetPosition(object.GetPosition());
SetRotation(object.GetRotation());
}
RigidBody3D::RigidBody3D(RigidBody3D&& object) noexcept :
m_geom(std::move(object.m_geom)),
m_body(std::move(object.m_body)),
m_forceAccumulator(std::move(object.m_forceAccumulator)),
m_torqueAccumulator(std::move(object.m_torqueAccumulator)),
m_world(object.m_world),
m_gravityFactor(object.m_gravityFactor),
m_mass(object.m_mass)
{
if (m_body)
NewtonBodySetUserData(m_body, this);
}
RigidBody3D::RigidBody3D(RigidBody3D&& object) noexcept = default;
RigidBody3D::~RigidBody3D()
{
@@ -84,16 +50,15 @@ namespace Nz
switch (coordSys)
{
case CoordSys::Global:
m_forceAccumulator += force;
WakeUp();
m_body->applyCentralForce(ToBullet(force));
break;
case CoordSys::Local:
m_forceAccumulator += GetRotation() * force;
WakeUp();
m_body->applyCentralForce(ToBullet(GetRotation() * force));
break;
}
// In case the body was sleeping, wake it up (force callback won't be called otherwise)
NewtonBodySetSleepState(m_body, 0);
}
void RigidBody3D::AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys)
@@ -101,19 +66,16 @@ namespace Nz
switch (coordSys)
{
case CoordSys::Global:
m_forceAccumulator += force;
m_torqueAccumulator += Vector3f::CrossProduct(point - GetMassCenter(CoordSys::Global), force);
WakeUp();
m_body->applyForce(ToBullet(force), ToBullet(point));
break;
case CoordSys::Local:
{
Matrix4f transformMatrix = GetMatrix();
return AddForce(transformMatrix.Transform(force, 0.f), transformMatrix.Transform(point), CoordSys::Global);
return AddForce(transformMatrix.Transform(force, 0.f), point, CoordSys::Global);
}
}
// In case the body was sleeping, wake it up (force callback won't be called otherwise)
NewtonBodySetSleepState(m_body, 0);
}
void RigidBody3D::AddTorque(const Vector3f& torque, CoordSys coordSys)
@@ -121,51 +83,45 @@ namespace Nz
switch (coordSys)
{
case CoordSys::Global:
m_torqueAccumulator += torque;
WakeUp();
m_body->applyTorque(ToBullet(torque));
break;
case CoordSys::Local:
Matrix4f transformMatrix = GetMatrix();
m_torqueAccumulator += transformMatrix.Transform(torque, 0.f);
WakeUp();
m_body->applyTorque(ToBullet(transformMatrix.Transform(torque, 0.f)));
break;
}
// In case the body was sleeping, wake it up (force callback won't be called otherwise)
NewtonBodySetSleepState(m_body, 0);
}
void RigidBody3D::EnableAutoSleep(bool autoSleep)
void RigidBody3D::EnableSleeping(bool enable)
{
NewtonBodySetAutoSleep(m_body, autoSleep);
m_body->setActivationState(DISABLE_DEACTIVATION);
}
void RigidBody3D::EnableSimulation(bool simulation)
void RigidBody3D::FallAsleep()
{
NewtonBodySetSimulationState(m_body, simulation);
if (m_body->getActivationState() != DISABLE_DEACTIVATION)
m_body->setActivationState(ISLAND_SLEEPING);
}
Boxf RigidBody3D::GetAABB() const
{
Vector3f min, max;
NewtonBodyGetAABB(m_body, &min.x, &max.x);
btVector3 min, max;
m_body->getAabb(min, max);
return Boxf(min, max);
return Boxf(FromBullet(min), FromBullet(max));
}
Vector3f RigidBody3D::GetAngularDamping() const
float RigidBody3D::GetAngularDamping() const
{
Vector3f angularDamping;
NewtonBodyGetAngularDamping(m_body, &angularDamping.x);
return angularDamping;
return m_body->getAngularDamping();
}
Vector3f RigidBody3D::GetAngularVelocity() const
{
Vector3f angularVelocity;
NewtonBodyGetOmega(m_body, &angularVelocity.x);
return angularVelocity;
return FromBullet(m_body->getAngularVelocity());
}
const std::shared_ptr<Collider3D>& RigidBody3D::GetGeom() const
@@ -173,88 +129,44 @@ namespace Nz
return m_geom;
}
float RigidBody3D::GetGravityFactor() const
{
return m_gravityFactor;
}
NewtonBody* RigidBody3D::GetHandle() const
{
return m_body;
}
float RigidBody3D::GetLinearDamping() const
{
return NewtonBodyGetLinearDamping(m_body);
return m_body->getLinearDamping();
}
Vector3f RigidBody3D::GetLinearVelocity() const
{
Vector3f velocity;
NewtonBodyGetVelocity(m_body, &velocity.x);
return velocity;
return FromBullet(m_body->getLinearVelocity());
}
float RigidBody3D::GetMass() const
{
return m_mass;
return m_body->getMass();
}
Vector3f RigidBody3D::GetMassCenter(CoordSys coordSys) const
{
Vector3f center;
NewtonBodyGetCentreOfMass(m_body, &center.x);
switch (coordSys)
{
case CoordSys::Global:
{
Matrix4f transformMatrix = GetMatrix();
center = transformMatrix.Transform(center);
break;
}
case CoordSys::Local:
break;
}
return center;
}
int RigidBody3D::GetMaterial() const
{
return NewtonBodyGetMaterialGroupID(m_body);
return FromBullet(m_body->getCenterOfMassPosition());
}
Matrix4f RigidBody3D::GetMatrix() const
{
Matrix4f matrix;
NewtonBodyGetMatrix(m_body, &matrix.m11);
return matrix;
return FromBullet(m_body->getWorldTransform());
}
Vector3f RigidBody3D::GetPosition() const
{
Vector3f pos;
NewtonBodyGetPosition(m_body, &pos.x);
return FromBullet(m_body->getWorldTransform().getOrigin());
}
return pos;
btRigidBody* RigidBody3D::GetRigidBody() const
{
return m_body.get();
}
Quaternionf RigidBody3D::GetRotation() const
{
// NewtonBodyGetRotation output X, Y, Z, W and Nz::Quaternion stores W, X, Y, Z so we use a temporary array to fix the order
std::array<float, 4> rot;
NewtonBodyGetRotation(m_body, rot.data());
return Quaternionf(rot[3], rot[0], rot[1], rot[2]);
}
void* RigidBody3D::GetUserdata() const
{
return m_userdata;
return FromBullet(m_body->getWorldTransform().getRotation());
}
PhysWorld3D* RigidBody3D::GetWorld() const
@@ -262,37 +174,32 @@ namespace Nz
return m_world;
}
bool RigidBody3D::IsAutoSleepEnabled() const
{
return NewtonBodyGetAutoSleep(m_body) != 0;
}
bool RigidBody3D::IsMoveable() const
{
return m_mass > 0.f;
}
bool RigidBody3D::IsSimulationEnabled() const
{
return NewtonBodyGetSimulationState(m_body) != 0;
return m_body->isActive();
}
bool RigidBody3D::IsSleeping() const
{
return NewtonBodyGetSleepState(m_body) != 0;
return m_body->getActivationState() == ISLAND_SLEEPING;
}
void RigidBody3D::SetAngularDamping(const Vector3f& angularDamping)
bool RigidBody3D::IsSleepingEnabled() const
{
NewtonBodySetAngularDamping(m_body, &angularDamping.x);
return m_body->getActivationState() != DISABLE_DEACTIVATION;
}
void RigidBody3D::SetAngularDamping(float angularDamping)
{
m_body->setDamping(m_body->getLinearDamping(), angularDamping);
}
void RigidBody3D::SetAngularVelocity(const Vector3f& angularVelocity)
{
NewtonBodySetOmega(m_body, &angularVelocity.x);
m_body->setAngularVelocity(ToBullet(angularVelocity));
}
void RigidBody3D::SetGeom(std::shared_ptr<Collider3D> geom)
void RigidBody3D::SetGeom(std::shared_ptr<Collider3D> geom, bool recomputeInertia)
{
if (m_geom != geom)
{
@@ -301,23 +208,27 @@ namespace Nz
else
m_geom = std::make_shared<NullCollider3D>();
NewtonBodySetCollision(m_body, m_geom->GetHandle(m_world));
}
}
m_body->setCollisionShape(m_geom->GetShape());
if (recomputeInertia)
{
float mass = GetMass();
void RigidBody3D::SetGravityFactor(float gravityFactor)
{
m_gravityFactor = gravityFactor;
Vector3f inertia;
m_geom->ComputeInertia(mass, &inertia);
m_body->setMassProps(mass, ToBullet(inertia));
}
}
}
void RigidBody3D::SetLinearDamping(float damping)
{
NewtonBodySetLinearDamping(m_body, damping);
m_body->setDamping(damping, m_body->getAngularDamping());
}
void RigidBody3D::SetLinearVelocity(const Vector3f& velocity)
{
NewtonBodySetVelocity(m_body, &velocity.x);
m_body->setLinearVelocity(ToBullet(velocity));
}
void RigidBody3D::SetMass(float mass)
@@ -325,94 +236,52 @@ namespace Nz
NazaraAssert(mass >= 0.f, "Mass must be positive and finite");
NazaraAssert(std::isfinite(mass), "Mass must be positive and finite");
if (m_mass > 0.f)
{
if (mass > 0.f)
{
// If we already have a mass, we already have an inertial matrix as well, just rescale it
float Ix, Iy, Iz;
NewtonBodyGetMass(m_body, &m_mass, &Ix, &Iy, &Iz);
Vector3f inertia;
m_geom->ComputeInertia(mass, &inertia);
float scale = mass / m_mass;
NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);
}
else
{
NewtonBodySetMassMatrix(m_body, 0.f, 0.f, 0.f, 0.f);
NewtonBodySetForceAndTorqueCallback(m_body, nullptr);
}
}
else
{
Vector3f inertia, origin;
m_geom->ComputeInertialMatrix(&inertia, &origin);
NewtonBodySetCentreOfMass(m_body, &origin.x);
NewtonBodySetMassMatrix(m_body, mass, inertia.x*mass, inertia.y*mass, inertia.z*mass);
NewtonBodySetForceAndTorqueCallback(m_body, &ForceAndTorqueCallback);
}
m_mass = mass;
m_body->setMassProps(mass, ToBullet(inertia));
}
void RigidBody3D::SetMassCenter(const Vector3f& center)
{
if (m_mass > 0.f)
NewtonBodySetCentreOfMass(m_body, &center.x);
}
btTransform centerTransform;
centerTransform.setIdentity();
centerTransform.setOrigin(ToBullet(center));
void RigidBody3D::SetMaterial(const std::string& materialName)
{
SetMaterial(m_world->GetMaterial(materialName));
}
void RigidBody3D::SetMaterial(int materialIndex)
{
NewtonBodySetMaterialGroupID(m_body, materialIndex);
m_body->setCenterOfMassTransform(centerTransform);
}
void RigidBody3D::SetPosition(const Vector3f& position)
{
Matrix4f transformMatrix = GetMatrix();
transformMatrix.SetTranslation(position);
btTransform worldTransform = m_body->getWorldTransform();
worldTransform.setOrigin(ToBullet(position));
UpdateBody(transformMatrix);
m_body->setWorldTransform(worldTransform);
}
void RigidBody3D::SetRotation(const Quaternionf& rotation)
{
Matrix4f transformMatrix = GetMatrix();
transformMatrix.SetRotation(rotation);
btTransform worldTransform = m_body->getWorldTransform();
worldTransform.setRotation(ToBullet(rotation));
UpdateBody(transformMatrix);
m_body->setWorldTransform(worldTransform);
}
void RigidBody3D::SetUserdata(void* ud)
void RigidBody3D::WakeUp()
{
m_userdata = ud;
}
m_body->setDeactivationTime(0);
RigidBody3D& RigidBody3D::operator=(const RigidBody3D& object)
{
RigidBody3D physObj(object);
return operator=(std::move(physObj));
if (m_body->getActivationState() == ISLAND_SLEEPING)
m_body->setActivationState(ACTIVE_TAG);
}
RigidBody3D& RigidBody3D::operator=(RigidBody3D&& object) noexcept
{
if (m_body)
NewtonDestroyBody(m_body);
Destroy();
m_body = std::move(object.m_body);
m_forceAccumulator = std::move(object.m_forceAccumulator);
m_geom = std::move(object.m_geom);
m_gravityFactor = object.m_gravityFactor;
m_mass = object.m_mass;
m_torqueAccumulator = std::move(object.m_torqueAccumulator);
m_world = object.m_world;
if (m_body)
NewtonBodySetUserData(m_body, this);
m_body = std::move(object.m_body);
m_geom = std::move(object.m_geom);
m_world = object.m_world;
return *this;
}
@@ -421,50 +290,10 @@ namespace Nz
{
if (m_body)
{
NewtonDestroyBody(m_body);
m_body = nullptr;
m_world->GetDynamicsWorld()->removeRigidBody(m_body.get());
m_body.reset();
}
m_geom.reset();
}
void RigidBody3D::UpdateBody(const Matrix4f& transformMatrix)
{
NewtonBodySetMatrix(m_body, &transformMatrix.m11);
if (NumberEquals(m_mass, 0.f))
{
// Moving a static body in Newton does not update bodies at the target location
// http://newtondynamics.com/wiki/index.php5?title=Can_i_dynamicly_move_a_TriMesh%3F
Vector3f min, max;
NewtonBodyGetAABB(m_body, &min.x, &max.x);
NewtonWorldForEachBodyInAABBDo(m_world->GetHandle(), &min.x, &max.x, [](const NewtonBody* const body, void* const userData) -> int
{
NazaraUnused(userData);
NewtonBodySetSleepState(body, 0);
return 1;
},
nullptr);
}
}
void RigidBody3D::ForceAndTorqueCallback(const NewtonBody* body, float timeStep, int threadIndex)
{
NazaraUnused(timeStep);
NazaraUnused(threadIndex);
RigidBody3D* me = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body));
if (!NumberEquals(me->m_gravityFactor, 0.f))
me->m_forceAccumulator += me->m_world->GetGravity() * me->m_gravityFactor * me->m_mass;
NewtonBodySetForce(body, &me->m_forceAccumulator.x);
NewtonBodySetTorque(body, &me->m_torqueAccumulator.x);
me->m_torqueAccumulator.Set(0.f);
me->m_forceAccumulator.Set(0.f);
///TODO: Implement gyroscopic force?
}
}