Switch from Nz prefix to namespace Nz
What a huge commit Former-commit-id: 38ac5eebf70adc1180f571f6006192d28fb99897
This commit is contained in:
Lynix
@@ -8,429 +8,432 @@
|
||||
#include <memory>
|
||||
#include <Nazara/Physics/Debug.hpp>
|
||||
|
||||
namespace
|
||||
namespace Nz
|
||||
{
|
||||
NzPhysGeomRef CreateGeomFromPrimitive(const NzPrimitive& primitive)
|
||||
namespace
|
||||
{
|
||||
switch (primitive.type)
|
||||
PhysGeomRef CreateGeomFromPrimitive(const Primitive& primitive)
|
||||
{
|
||||
case nzPrimitiveType_Box:
|
||||
return NzBoxGeom::New(primitive.box.lengths, primitive.matrix);
|
||||
switch (primitive.type)
|
||||
{
|
||||
case PrimitiveType_Box:
|
||||
return BoxGeom::New(primitive.box.lengths, primitive.matrix);
|
||||
|
||||
case nzPrimitiveType_Cone:
|
||||
return NzConeGeom::New(primitive.cone.length, primitive.cone.radius, primitive.matrix);
|
||||
case PrimitiveType_Cone:
|
||||
return ConeGeom::New(primitive.cone.length, primitive.cone.radius, primitive.matrix);
|
||||
|
||||
case nzPrimitiveType_Plane:
|
||||
return NzBoxGeom::New(NzVector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y), primitive.matrix);
|
||||
///TODO: PlaneGeom?
|
||||
case PrimitiveType_Plane:
|
||||
return BoxGeom::New(Vector3f(primitive.plane.size.x, 0.01f, primitive.plane.size.y), primitive.matrix);
|
||||
///TODO: PlaneGeom?
|
||||
|
||||
case nzPrimitiveType_Sphere:
|
||||
return NzSphereGeom::New(primitive.sphere.size, primitive.matrix.GetTranslation());
|
||||
case PrimitiveType_Sphere:
|
||||
return SphereGeom::New(primitive.sphere.size, primitive.matrix.GetTranslation());
|
||||
}
|
||||
|
||||
NazaraError("Primitive type not handled (0x" + String::Number(primitive.type, 16) + ')');
|
||||
return PhysGeomRef();
|
||||
}
|
||||
|
||||
NazaraError("Primitive type not handled (0x" + NzString::Number(primitive.type, 16) + ')');
|
||||
return NzPhysGeomRef();
|
||||
}
|
||||
}
|
||||
|
||||
NzPhysGeom::~NzPhysGeom()
|
||||
{
|
||||
for (auto& pair : m_handles)
|
||||
NewtonDestroyCollision(pair.second);
|
||||
}
|
||||
|
||||
NzBoxf NzPhysGeom::ComputeAABB(const NzVector3f& translation, const NzQuaternionf& rotation, const NzVector3f& scale) const
|
||||
{
|
||||
return ComputeAABB(NzMatrix4f::Transform(translation, rotation), scale);
|
||||
}
|
||||
|
||||
NzBoxf NzPhysGeom::ComputeAABB(const NzMatrix4f& offsetMatrix, const NzVector3f& scale) const
|
||||
{
|
||||
NzVector3f min, max;
|
||||
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
PhysGeom::~PhysGeom()
|
||||
{
|
||||
NzPhysWorld world;
|
||||
for (auto& pair : m_handles)
|
||||
NewtonDestroyCollision(pair.second);
|
||||
}
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
Boxf PhysGeom::ComputeAABB(const Vector3f& translation, const Quaternionf& rotation, const Vector3f& scale) const
|
||||
{
|
||||
return ComputeAABB(Matrix4f::Transform(translation, rotation), scale);
|
||||
}
|
||||
|
||||
Boxf PhysGeom::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) const
|
||||
{
|
||||
Vector3f min, max;
|
||||
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
{
|
||||
NewtonCollisionCalculateAABB(collision, offsetMatrix, min, max);
|
||||
PhysWorld world;
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
{
|
||||
NewtonCollisionCalculateAABB(collision, offsetMatrix, min, max);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
NewtonCollisionCalculateAABB(m_handles.begin()->second, offsetMatrix, min, max);
|
||||
|
||||
return Boxf(scale * min, scale * max);
|
||||
}
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
NewtonCollisionCalculateAABB(m_handles.begin()->second, offsetMatrix, min, max);
|
||||
|
||||
return NzBoxf(scale * min, scale * max);
|
||||
}
|
||||
|
||||
void NzPhysGeom::ComputeInertialMatrix(NzVector3f* inertia, NzVector3f* center) const
|
||||
{
|
||||
float inertiaMatrix[3];
|
||||
float origin[3];
|
||||
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
void PhysGeom::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
|
||||
{
|
||||
NzPhysWorld world;
|
||||
float inertiaMatrix[3];
|
||||
float origin[3];
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
{
|
||||
NewtonConvexCollisionCalculateInertialMatrix(collision, inertiaMatrix, origin);
|
||||
PhysWorld world;
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
{
|
||||
NewtonConvexCollisionCalculateInertialMatrix(collision, inertiaMatrix, origin);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
NewtonConvexCollisionCalculateInertialMatrix(m_handles.begin()->second, inertiaMatrix, origin);
|
||||
|
||||
if (inertia)
|
||||
inertia->Set(inertiaMatrix);
|
||||
|
||||
if (center)
|
||||
center->Set(origin);
|
||||
}
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
NewtonConvexCollisionCalculateInertialMatrix(m_handles.begin()->second, inertiaMatrix, origin);
|
||||
|
||||
if (inertia)
|
||||
inertia->Set(inertiaMatrix);
|
||||
|
||||
if (center)
|
||||
center->Set(origin);
|
||||
}
|
||||
|
||||
float NzPhysGeom::ComputeVolume() const
|
||||
{
|
||||
float volume;
|
||||
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
float PhysGeom::ComputeVolume() const
|
||||
{
|
||||
NzPhysWorld world;
|
||||
float volume;
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
// Si nous n'avons aucune instance, nous en créons une temporaire
|
||||
if (m_handles.empty())
|
||||
{
|
||||
volume = NewtonConvexCollisionCalculateVolume(collision);
|
||||
PhysWorld world;
|
||||
|
||||
NewtonCollision* collision = CreateHandle(&world);
|
||||
{
|
||||
volume = NewtonConvexCollisionCalculateVolume(collision);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
}
|
||||
NewtonDestroyCollision(collision);
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
volume = NewtonConvexCollisionCalculateVolume(m_handles.begin()->second);
|
||||
|
||||
return volume;
|
||||
}
|
||||
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
|
||||
volume = NewtonConvexCollisionCalculateVolume(m_handles.begin()->second);
|
||||
|
||||
return volume;
|
||||
}
|
||||
|
||||
NewtonCollision* NzPhysGeom::GetHandle(NzPhysWorld* 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;
|
||||
}
|
||||
|
||||
NzPhysGeomRef NzPhysGeom::Build(const NzPrimitiveList& list)
|
||||
{
|
||||
unsigned int primitiveCount = list.GetSize();
|
||||
|
||||
#if NAZARA_PHYSICS_SAFE
|
||||
if (primitiveCount == 0)
|
||||
NewtonCollision* PhysGeom::GetHandle(PhysWorld* world) const
|
||||
{
|
||||
NazaraError("PrimitiveList must have at least one primitive");
|
||||
return nullptr;
|
||||
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;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (primitiveCount > 1)
|
||||
PhysGeomRef PhysGeom::Build(const PrimitiveList& list)
|
||||
{
|
||||
std::vector<NzPhysGeom*> geoms(primitiveCount);
|
||||
unsigned int primitiveCount = list.GetSize();
|
||||
|
||||
for (unsigned int i = 0; i < primitiveCount; ++i)
|
||||
geoms[i] = CreateGeomFromPrimitive(list.GetPrimitive(i));
|
||||
|
||||
return NzCompoundGeom::New(&geoms[0], primitiveCount);
|
||||
}
|
||||
else
|
||||
return CreateGeomFromPrimitive(list.GetPrimitive(0));
|
||||
}
|
||||
|
||||
NzPhysGeomLibrary::LibraryMap NzPhysGeom::s_library;
|
||||
|
||||
/********************************** BoxGeom **********************************/
|
||||
|
||||
NzBoxGeom::NzBoxGeom(const NzVector3f& lengths, const NzMatrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_lengths(lengths)
|
||||
{
|
||||
}
|
||||
|
||||
NzBoxGeom::NzBoxGeom(const NzVector3f& lengths, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
NzBoxGeom(lengths, NzMatrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
NzBoxf NzBoxGeom::ComputeAABB(const NzMatrix4f& offsetMatrix, const NzVector3f& scale) const
|
||||
{
|
||||
NzVector3f halfLengths(m_lengths * 0.5f);
|
||||
|
||||
NzBoxf aabb(-halfLengths.x, -halfLengths.y, -halfLengths.z, m_lengths.x, m_lengths.y, m_lengths.z);
|
||||
aabb.Transform(offsetMatrix, true);
|
||||
aabb *= scale;
|
||||
|
||||
return aabb;
|
||||
}
|
||||
|
||||
float NzBoxGeom::ComputeVolume() const
|
||||
{
|
||||
return m_lengths.x * m_lengths.y * m_lengths.z;
|
||||
}
|
||||
|
||||
NzVector3f NzBoxGeom::GetLengths() const
|
||||
{
|
||||
return m_lengths;
|
||||
}
|
||||
|
||||
nzGeomType NzBoxGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Box;
|
||||
}
|
||||
|
||||
NewtonCollision* NzBoxGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateBox(world->GetHandle(), m_lengths.x, m_lengths.y, m_lengths.z, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************** CapsuleGeom ********************************/
|
||||
|
||||
NzCapsuleGeom::NzCapsuleGeom(float length, float radius, const NzMatrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
NzCapsuleGeom::NzCapsuleGeom(float length, float radius, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
NzCapsuleGeom(length, radius, NzMatrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float NzCapsuleGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float NzCapsuleGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
nzGeomType NzCapsuleGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Capsule;
|
||||
}
|
||||
|
||||
NewtonCollision* NzCapsuleGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCapsule(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************* CompoundGeom ********************************/
|
||||
|
||||
NzCompoundGeom::NzCompoundGeom(NzPhysGeom** geoms, unsigned int geomCount)
|
||||
{
|
||||
m_geoms.reserve(geomCount);
|
||||
for (unsigned int i = 0; i < geomCount; ++i)
|
||||
m_geoms.emplace_back(geoms[i]);
|
||||
}
|
||||
|
||||
const std::vector<NzPhysGeomRef>& NzCompoundGeom::GetGeoms() const
|
||||
{
|
||||
return m_geoms;
|
||||
}
|
||||
|
||||
nzGeomType NzCompoundGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Compound;
|
||||
}
|
||||
|
||||
NewtonCollision* NzCompoundGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
NewtonCollision* compoundCollision = NewtonCreateCompoundCollision(world->GetHandle(), 0);
|
||||
|
||||
NewtonCompoundCollisionBeginAddRemove(compoundCollision);
|
||||
for (const NzPhysGeomRef& geom : m_geoms)
|
||||
{
|
||||
if (geom->GetType() == nzGeomType_Compound)
|
||||
#if NAZARA_PHYSICS_SAFE
|
||||
if (primitiveCount == 0)
|
||||
{
|
||||
NzCompoundGeom* compoundGeom = static_cast<NzCompoundGeom*>(geom.Get());
|
||||
for (const NzPhysGeomRef& piece : compoundGeom->GetGeoms())
|
||||
NewtonCompoundCollisionAddSubCollision(compoundCollision, piece->GetHandle(world));
|
||||
NazaraError("PrimitiveList must have at least one primitive");
|
||||
return nullptr;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (primitiveCount > 1)
|
||||
{
|
||||
std::vector<PhysGeom*> geoms(primitiveCount);
|
||||
|
||||
for (unsigned int i = 0; i < primitiveCount; ++i)
|
||||
geoms[i] = CreateGeomFromPrimitive(list.GetPrimitive(i));
|
||||
|
||||
return CompoundGeom::New(&geoms[0], primitiveCount);
|
||||
}
|
||||
else
|
||||
NewtonCompoundCollisionAddSubCollision(compoundCollision, geom->GetHandle(world));
|
||||
return CreateGeomFromPrimitive(list.GetPrimitive(0));
|
||||
}
|
||||
NewtonCompoundCollisionEndAddRemove(compoundCollision);
|
||||
|
||||
return compoundCollision;
|
||||
}
|
||||
PhysGeomLibrary::LibraryMap PhysGeom::s_library;
|
||||
|
||||
/********************************* ConeGeom **********************************/
|
||||
/********************************** BoxGeom **********************************/
|
||||
|
||||
NzConeGeom::NzConeGeom(float length, float radius, const NzMatrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
NzConeGeom::NzConeGeom(float length, float radius, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
NzConeGeom(length, radius, NzMatrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float NzConeGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float NzConeGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
nzGeomType NzConeGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Cone;
|
||||
}
|
||||
|
||||
NewtonCollision* NzConeGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCone(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/****************************** ConvexHullGeom *******************************/
|
||||
|
||||
NzConvexHullGeom::NzConvexHullGeom(const void* vertices, unsigned int vertexCount, unsigned int stride, float tolerance, const NzMatrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_tolerance(tolerance),
|
||||
m_vertexStride(stride)
|
||||
{
|
||||
const nzUInt8* ptr = static_cast<const nzUInt8*>(vertices);
|
||||
|
||||
m_vertices.resize(vertexCount);
|
||||
if (stride != sizeof(NzVector3f))
|
||||
BoxGeom::BoxGeom(const Vector3f& lengths, const Matrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_lengths(lengths)
|
||||
{
|
||||
for (unsigned int i = 0; i < vertexCount; ++i)
|
||||
m_vertices[i] = *reinterpret_cast<const NzVector3f*>(ptr + stride*i);
|
||||
}
|
||||
else // Fast path
|
||||
std::memcpy(m_vertices.data(), vertices, vertexCount*sizeof(NzVector3f));
|
||||
}
|
||||
|
||||
NzConvexHullGeom::NzConvexHullGeom(const void* vertices, unsigned int vertexCount, unsigned int stride, float tolerance, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
NzConvexHullGeom(vertices, vertexCount, stride, tolerance, NzMatrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
nzGeomType NzConvexHullGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Compound;
|
||||
}
|
||||
|
||||
NewtonCollision* NzConvexHullGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateConvexHull(world->GetHandle(), m_vertices.size(), reinterpret_cast<const float*>(m_vertices.data()), sizeof(NzVector3f), m_tolerance, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************* CylinderGeom ********************************/
|
||||
|
||||
NzCylinderGeom::NzCylinderGeom(float length, float radius, const NzMatrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
NzCylinderGeom::NzCylinderGeom(float length, float radius, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
NzCylinderGeom(length, radius, NzMatrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float NzCylinderGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float NzCylinderGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
nzGeomType NzCylinderGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Cylinder;
|
||||
}
|
||||
|
||||
NewtonCollision* NzCylinderGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCylinder(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/********************************* NullGeom **********************************/
|
||||
|
||||
NzNullGeom::NzNullGeom()
|
||||
{
|
||||
}
|
||||
|
||||
nzGeomType NzNullGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Null;
|
||||
}
|
||||
|
||||
void NzNullGeom::ComputeInertialMatrix(NzVector3f* inertia, NzVector3f* center) const
|
||||
{
|
||||
if (inertia)
|
||||
inertia->MakeUnit();
|
||||
|
||||
if (center)
|
||||
center->MakeZero();
|
||||
}
|
||||
|
||||
NewtonCollision* NzNullGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateNull(world->GetHandle());
|
||||
}
|
||||
|
||||
/******************************** SphereGeom *********************************/
|
||||
|
||||
NzSphereGeom::NzSphereGeom(float radius, const NzMatrix4f& transformMatrix) :
|
||||
NzSphereGeom(radius, transformMatrix.GetTranslation())
|
||||
{
|
||||
}
|
||||
|
||||
NzSphereGeom::NzSphereGeom(float radius, const NzVector3f& translation, const NzQuaternionf& rotation) :
|
||||
m_position(translation),
|
||||
m_radius(radius)
|
||||
{
|
||||
NazaraUnused(rotation);
|
||||
}
|
||||
|
||||
NzBoxf NzSphereGeom::ComputeAABB(const NzMatrix4f& offsetMatrix, const NzVector3f& scale) const
|
||||
{
|
||||
NzVector3f size(m_radius * NazaraSuffixMacro(M_SQRT3, f) * scale);
|
||||
NzVector3f position(offsetMatrix.GetTranslation());
|
||||
|
||||
return NzBoxf(position - size, position + size);
|
||||
}
|
||||
|
||||
float NzSphereGeom::ComputeVolume() const
|
||||
{
|
||||
return float(M_PI) * m_radius * m_radius * m_radius / 3.f;
|
||||
}
|
||||
|
||||
float NzSphereGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
nzGeomType NzSphereGeom::GetType() const
|
||||
{
|
||||
return nzGeomType_Sphere;
|
||||
}
|
||||
|
||||
NewtonCollision* NzSphereGeom::CreateHandle(NzPhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateSphere(world->GetHandle(), m_radius, 0, NzMatrix4f::Translate(m_position));
|
||||
|
||||
BoxGeom::BoxGeom(const Vector3f& lengths, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
BoxGeom(lengths, Matrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
Boxf BoxGeom::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) 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;
|
||||
|
||||
return aabb;
|
||||
}
|
||||
|
||||
float BoxGeom::ComputeVolume() const
|
||||
{
|
||||
return m_lengths.x * m_lengths.y * m_lengths.z;
|
||||
}
|
||||
|
||||
Vector3f BoxGeom::GetLengths() const
|
||||
{
|
||||
return m_lengths;
|
||||
}
|
||||
|
||||
GeomType BoxGeom::GetType() const
|
||||
{
|
||||
return GeomType_Box;
|
||||
}
|
||||
|
||||
NewtonCollision* BoxGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateBox(world->GetHandle(), m_lengths.x, m_lengths.y, m_lengths.z, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************** CapsuleGeom ********************************/
|
||||
|
||||
CapsuleGeom::CapsuleGeom(float length, float radius, const Matrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
CapsuleGeom::CapsuleGeom(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
CapsuleGeom(length, radius, Matrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float CapsuleGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float CapsuleGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
GeomType CapsuleGeom::GetType() const
|
||||
{
|
||||
return GeomType_Capsule;
|
||||
}
|
||||
|
||||
NewtonCollision* CapsuleGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCapsule(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************* CompoundGeom ********************************/
|
||||
|
||||
CompoundGeom::CompoundGeom(PhysGeom** geoms, unsigned int geomCount)
|
||||
{
|
||||
m_geoms.reserve(geomCount);
|
||||
for (unsigned int i = 0; i < geomCount; ++i)
|
||||
m_geoms.emplace_back(geoms[i]);
|
||||
}
|
||||
|
||||
const std::vector<PhysGeomRef>& CompoundGeom::GetGeoms() const
|
||||
{
|
||||
return m_geoms;
|
||||
}
|
||||
|
||||
GeomType CompoundGeom::GetType() const
|
||||
{
|
||||
return GeomType_Compound;
|
||||
}
|
||||
|
||||
NewtonCollision* CompoundGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
NewtonCollision* compoundCollision = NewtonCreateCompoundCollision(world->GetHandle(), 0);
|
||||
|
||||
NewtonCompoundCollisionBeginAddRemove(compoundCollision);
|
||||
for (const PhysGeomRef& geom : m_geoms)
|
||||
{
|
||||
if (geom->GetType() == GeomType_Compound)
|
||||
{
|
||||
CompoundGeom* compoundGeom = static_cast<CompoundGeom*>(geom.Get());
|
||||
for (const PhysGeomRef& piece : compoundGeom->GetGeoms())
|
||||
NewtonCompoundCollisionAddSubCollision(compoundCollision, piece->GetHandle(world));
|
||||
}
|
||||
else
|
||||
NewtonCompoundCollisionAddSubCollision(compoundCollision, geom->GetHandle(world));
|
||||
}
|
||||
NewtonCompoundCollisionEndAddRemove(compoundCollision);
|
||||
|
||||
return compoundCollision;
|
||||
}
|
||||
|
||||
/********************************* ConeGeom **********************************/
|
||||
|
||||
ConeGeom::ConeGeom(float length, float radius, const Matrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
ConeGeom::ConeGeom(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
ConeGeom(length, radius, Matrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float ConeGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float ConeGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
GeomType ConeGeom::GetType() const
|
||||
{
|
||||
return GeomType_Cone;
|
||||
}
|
||||
|
||||
NewtonCollision* ConeGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCone(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/****************************** ConvexHullGeom *******************************/
|
||||
|
||||
ConvexHullGeom::ConvexHullGeom(const void* vertices, unsigned int vertexCount, unsigned int stride, float tolerance, const Matrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_tolerance(tolerance),
|
||||
m_vertexStride(stride)
|
||||
{
|
||||
const UInt8* ptr = static_cast<const UInt8*>(vertices);
|
||||
|
||||
m_vertices.resize(vertexCount);
|
||||
if (stride != sizeof(Vector3f))
|
||||
{
|
||||
for (unsigned int i = 0; i < vertexCount; ++i)
|
||||
m_vertices[i] = *reinterpret_cast<const Vector3f*>(ptr + stride*i);
|
||||
}
|
||||
else // Fast path
|
||||
std::memcpy(m_vertices.data(), vertices, vertexCount*sizeof(Vector3f));
|
||||
}
|
||||
|
||||
ConvexHullGeom::ConvexHullGeom(const void* vertices, unsigned int vertexCount, unsigned int stride, float tolerance, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
ConvexHullGeom(vertices, vertexCount, stride, tolerance, Matrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
GeomType ConvexHullGeom::GetType() const
|
||||
{
|
||||
return GeomType_Compound;
|
||||
}
|
||||
|
||||
NewtonCollision* ConvexHullGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateConvexHull(world->GetHandle(), m_vertices.size(), reinterpret_cast<const float*>(m_vertices.data()), sizeof(Vector3f), m_tolerance, 0, m_matrix);
|
||||
}
|
||||
|
||||
/******************************* CylinderGeom ********************************/
|
||||
|
||||
NzCylinderGeom::NzCylinderGeom(float length, float radius, const Matrix4f& transformMatrix) :
|
||||
m_matrix(transformMatrix),
|
||||
m_length(length),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
NzCylinderGeom::NzCylinderGeom(float length, float radius, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
NzCylinderGeom(length, radius, Matrix4f::Transform(translation, rotation))
|
||||
{
|
||||
}
|
||||
|
||||
float NzCylinderGeom::GetLength() const
|
||||
{
|
||||
return m_length;
|
||||
}
|
||||
|
||||
float NzCylinderGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
GeomType NzCylinderGeom::GetType() const
|
||||
{
|
||||
return GeomType_Cylinder;
|
||||
}
|
||||
|
||||
NewtonCollision* NzCylinderGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateCylinder(world->GetHandle(), m_radius, m_length, 0, m_matrix);
|
||||
}
|
||||
|
||||
/********************************* NullGeom **********************************/
|
||||
|
||||
NullGeom::NullGeom()
|
||||
{
|
||||
}
|
||||
|
||||
GeomType NullGeom::GetType() const
|
||||
{
|
||||
return GeomType_Null;
|
||||
}
|
||||
|
||||
void NullGeom::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
|
||||
{
|
||||
if (inertia)
|
||||
inertia->MakeUnit();
|
||||
|
||||
if (center)
|
||||
center->MakeZero();
|
||||
}
|
||||
|
||||
NewtonCollision* NullGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateNull(world->GetHandle());
|
||||
}
|
||||
|
||||
/******************************** SphereGeom *********************************/
|
||||
|
||||
SphereGeom::SphereGeom(float radius, const Matrix4f& transformMatrix) :
|
||||
SphereGeom(radius, transformMatrix.GetTranslation())
|
||||
{
|
||||
}
|
||||
|
||||
SphereGeom::SphereGeom(float radius, const Vector3f& translation, const Quaternionf& rotation) :
|
||||
m_position(translation),
|
||||
m_radius(radius)
|
||||
{
|
||||
NazaraUnused(rotation);
|
||||
}
|
||||
|
||||
Boxf SphereGeom::ComputeAABB(const Matrix4f& offsetMatrix, const Vector3f& scale) const
|
||||
{
|
||||
Vector3f size(m_radius * NazaraSuffixMacro(M_SQRT3, f) * scale);
|
||||
Vector3f position(offsetMatrix.GetTranslation());
|
||||
|
||||
return Boxf(position - size, position + size);
|
||||
}
|
||||
|
||||
float SphereGeom::ComputeVolume() const
|
||||
{
|
||||
return float(M_PI) * m_radius * m_radius * m_radius / 3.f;
|
||||
}
|
||||
|
||||
float SphereGeom::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
GeomType SphereGeom::GetType() const
|
||||
{
|
||||
return GeomType_Sphere;
|
||||
}
|
||||
|
||||
NewtonCollision* SphereGeom::CreateHandle(PhysWorld* world) const
|
||||
{
|
||||
return NewtonCreateSphere(world->GetHandle(), m_radius, 0, Matrix4f::Translate(m_position));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,356 +10,359 @@
|
||||
#include <algorithm>
|
||||
#include <Nazara/Physics/Debug.hpp>
|
||||
|
||||
NzPhysObject::NzPhysObject(NzPhysWorld* world, const NzMatrix4f& mat) :
|
||||
NzPhysObject(world, NzNullGeom::New(), mat)
|
||||
namespace Nz
|
||||
{
|
||||
}
|
||||
|
||||
NzPhysObject::NzPhysObject(NzPhysWorld* world, NzPhysGeomRef geom, const NzMatrix4f& mat) :
|
||||
m_matrix(mat),
|
||||
m_geom(std::move(geom)),
|
||||
m_forceAccumulator(NzVector3f::Zero()),
|
||||
m_torqueAccumulator(NzVector3f::Zero()),
|
||||
m_world(world),
|
||||
m_gravityFactor(1.f),
|
||||
m_mass(0.f)
|
||||
{
|
||||
NazaraAssert(m_world, "Invalid world");
|
||||
|
||||
if (!m_geom)
|
||||
m_geom = NzNullGeom::New();
|
||||
|
||||
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), m_matrix);
|
||||
NewtonBodySetUserData(m_body, this);
|
||||
}
|
||||
|
||||
NzPhysObject::NzPhysObject(const NzPhysObject& object) :
|
||||
m_matrix(object.m_matrix),
|
||||
m_geom(object.m_geom),
|
||||
m_forceAccumulator(NzVector3f::Zero()),
|
||||
m_torqueAccumulator(NzVector3f::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");
|
||||
|
||||
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), m_matrix);
|
||||
NewtonBodySetUserData(m_body, this);
|
||||
SetMass(object.m_mass);
|
||||
}
|
||||
|
||||
NzPhysObject::NzPhysObject(NzPhysObject&& object) :
|
||||
m_matrix(std::move(object.m_matrix)),
|
||||
m_geom(std::move(object.m_geom)),
|
||||
m_forceAccumulator(std::move(object.m_forceAccumulator)),
|
||||
m_torqueAccumulator(std::move(object.m_torqueAccumulator)),
|
||||
m_body(object.m_body),
|
||||
m_world(object.m_world),
|
||||
m_gravityFactor(object.m_gravityFactor),
|
||||
m_mass(object.m_mass)
|
||||
{
|
||||
object.m_body = nullptr;
|
||||
}
|
||||
|
||||
NzPhysObject::~NzPhysObject()
|
||||
{
|
||||
if (m_body)
|
||||
NewtonDestroyBody(m_world->GetHandle(), m_body);
|
||||
}
|
||||
|
||||
void NzPhysObject::AddForce(const NzVector3f& force, nzCoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
PhysObject::PhysObject(PhysWorld* world, const Matrix4f& mat) :
|
||||
PhysObject(world, NullGeom::New(), mat)
|
||||
{
|
||||
case nzCoordSys_Global:
|
||||
m_forceAccumulator += force;
|
||||
break;
|
||||
|
||||
case nzCoordSys_Local:
|
||||
m_forceAccumulator += GetRotation() * force;
|
||||
break;
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void NzPhysObject::AddForce(const NzVector3f& force, const NzVector3f& point, nzCoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
PhysObject::PhysObject(PhysWorld* world, PhysGeomRef geom, const Matrix4f& mat) :
|
||||
m_matrix(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)
|
||||
{
|
||||
case nzCoordSys_Global:
|
||||
m_forceAccumulator += force;
|
||||
m_torqueAccumulator += NzVector3f::CrossProduct(point - GetMassCenter(nzCoordSys_Global), force);
|
||||
break;
|
||||
NazaraAssert(m_world, "Invalid world");
|
||||
|
||||
case nzCoordSys_Local:
|
||||
return AddForce(m_matrix.Transform(force, 0.f), m_matrix.Transform(point), nzCoordSys_Global);
|
||||
if (!m_geom)
|
||||
m_geom = NullGeom::New();
|
||||
|
||||
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), m_matrix);
|
||||
NewtonBodySetUserData(m_body, this);
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void NzPhysObject::AddTorque(const NzVector3f& torque, nzCoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
PhysObject::PhysObject(const PhysObject& object) :
|
||||
m_matrix(object.m_matrix),
|
||||
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)
|
||||
{
|
||||
case nzCoordSys_Global:
|
||||
m_torqueAccumulator += torque;
|
||||
break;
|
||||
NazaraAssert(m_world, "Invalid world");
|
||||
NazaraAssert(m_geom, "Invalid geometry");
|
||||
|
||||
case nzCoordSys_Local:
|
||||
m_torqueAccumulator += m_matrix.Transform(torque, 0.f);
|
||||
break;
|
||||
m_body = NewtonCreateDynamicBody(m_world->GetHandle(), m_geom->GetHandle(m_world), m_matrix);
|
||||
NewtonBodySetUserData(m_body, this);
|
||||
SetMass(object.m_mass);
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void NzPhysObject::EnableAutoSleep(bool autoSleep)
|
||||
{
|
||||
NewtonBodySetAutoSleep(m_body, autoSleep);
|
||||
}
|
||||
|
||||
NzBoxf NzPhysObject::GetAABB() const
|
||||
{
|
||||
NzVector3f min, max;
|
||||
NewtonBodyGetAABB(m_body, min, max);
|
||||
|
||||
return NzBoxf(min, max);
|
||||
}
|
||||
|
||||
NzVector3f NzPhysObject::GetAngularVelocity() const
|
||||
{
|
||||
NzVector3f angularVelocity;
|
||||
NewtonBodyGetOmega(m_body, angularVelocity);
|
||||
|
||||
return angularVelocity;
|
||||
}
|
||||
|
||||
const NzPhysGeomRef& NzPhysObject::GetGeom() const
|
||||
{
|
||||
return m_geom;
|
||||
}
|
||||
|
||||
float NzPhysObject::GetGravityFactor() const
|
||||
{
|
||||
return m_gravityFactor;
|
||||
}
|
||||
|
||||
NewtonBody* NzPhysObject::GetHandle() const
|
||||
{
|
||||
return m_body;
|
||||
}
|
||||
|
||||
float NzPhysObject::GetMass() const
|
||||
{
|
||||
return m_mass;
|
||||
}
|
||||
|
||||
NzVector3f NzPhysObject::GetMassCenter(nzCoordSys coordSys) const
|
||||
{
|
||||
NzVector3f center;
|
||||
NewtonBodyGetCentreOfMass(m_body, center);
|
||||
|
||||
switch (coordSys)
|
||||
PhysObject::PhysObject(PhysObject&& object) :
|
||||
m_matrix(std::move(object.m_matrix)),
|
||||
m_geom(std::move(object.m_geom)),
|
||||
m_forceAccumulator(std::move(object.m_forceAccumulator)),
|
||||
m_torqueAccumulator(std::move(object.m_torqueAccumulator)),
|
||||
m_body(object.m_body),
|
||||
m_world(object.m_world),
|
||||
m_gravityFactor(object.m_gravityFactor),
|
||||
m_mass(object.m_mass)
|
||||
{
|
||||
case nzCoordSys_Global:
|
||||
center = m_matrix.Transform(center);
|
||||
break;
|
||||
|
||||
case nzCoordSys_Local:
|
||||
break; // Aucune opération à effectuer sur le centre de rotation
|
||||
object.m_body = nullptr;
|
||||
}
|
||||
|
||||
return center;
|
||||
}
|
||||
|
||||
const NzMatrix4f& NzPhysObject::GetMatrix() const
|
||||
{
|
||||
return m_matrix;
|
||||
}
|
||||
|
||||
NzVector3f NzPhysObject::GetPosition() const
|
||||
{
|
||||
return m_matrix.GetTranslation();
|
||||
}
|
||||
|
||||
NzQuaternionf NzPhysObject::GetRotation() const
|
||||
{
|
||||
return m_matrix.GetRotation();
|
||||
}
|
||||
|
||||
NzVector3f NzPhysObject::GetVelocity() const
|
||||
{
|
||||
NzVector3f velocity;
|
||||
NewtonBodyGetVelocity(m_body, velocity);
|
||||
|
||||
return velocity;
|
||||
}
|
||||
|
||||
bool NzPhysObject::IsAutoSleepEnabled() const
|
||||
{
|
||||
return NewtonBodyGetAutoSleep(m_body) != 0;
|
||||
}
|
||||
|
||||
bool NzPhysObject::IsMoveable() const
|
||||
{
|
||||
return m_mass > 0.f;
|
||||
}
|
||||
|
||||
bool NzPhysObject::IsSleeping() const
|
||||
{
|
||||
return NewtonBodyGetSleepState(m_body) != 0;
|
||||
}
|
||||
|
||||
void NzPhysObject::SetAngularVelocity(const NzVector3f& angularVelocity)
|
||||
{
|
||||
NewtonBodySetOmega(m_body, angularVelocity);
|
||||
}
|
||||
|
||||
void NzPhysObject::SetGeom(NzPhysGeomRef geom)
|
||||
{
|
||||
if (m_geom.Get() != geom)
|
||||
PhysObject::~PhysObject()
|
||||
{
|
||||
if (geom)
|
||||
m_geom = geom;
|
||||
else
|
||||
m_geom = NzNullGeom::New();
|
||||
|
||||
NewtonBodySetCollision(m_body, m_geom->GetHandle(m_world));
|
||||
}
|
||||
}
|
||||
|
||||
void NzPhysObject::SetGravityFactor(float gravityFactor)
|
||||
{
|
||||
m_gravityFactor = gravityFactor;
|
||||
}
|
||||
|
||||
void NzPhysObject::SetMass(float mass)
|
||||
{
|
||||
if (m_mass > 0.f)
|
||||
{
|
||||
float Ix, Iy, Iz;
|
||||
NewtonBodyGetMassMatrix(m_body, &m_mass, &Ix, &Iy, &Iz);
|
||||
float scale = mass/m_mass;
|
||||
NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);
|
||||
}
|
||||
else if (mass > 0.f)
|
||||
{
|
||||
NzVector3f 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);
|
||||
NewtonBodySetTransformCallback(m_body, &TransformCallback);
|
||||
if (m_body)
|
||||
NewtonDestroyBody(m_world->GetHandle(), m_body);
|
||||
}
|
||||
|
||||
m_mass = mass;
|
||||
}
|
||||
|
||||
void NzPhysObject::SetMassCenter(const NzVector3f& center)
|
||||
{
|
||||
if (m_mass > 0.f)
|
||||
NewtonBodySetCentreOfMass(m_body, center);
|
||||
}
|
||||
|
||||
void NzPhysObject::SetPosition(const NzVector3f& position)
|
||||
{
|
||||
m_matrix.SetTranslation(position);
|
||||
UpdateBody();
|
||||
}
|
||||
|
||||
void NzPhysObject::SetRotation(const NzQuaternionf& rotation)
|
||||
{
|
||||
m_matrix.SetRotation(rotation);
|
||||
UpdateBody();
|
||||
}
|
||||
|
||||
void NzPhysObject::SetVelocity(const NzVector3f& velocity)
|
||||
{
|
||||
NewtonBodySetVelocity(m_body, velocity);
|
||||
}
|
||||
|
||||
NzPhysObject& NzPhysObject::operator=(const NzPhysObject& object)
|
||||
{
|
||||
NzPhysObject physObj(object);
|
||||
return operator=(std::move(physObj));
|
||||
}
|
||||
|
||||
void NzPhysObject::UpdateBody()
|
||||
{
|
||||
NewtonBodySetMatrix(m_body, m_matrix);
|
||||
|
||||
if (NzNumberEquals(m_mass, 0.f))
|
||||
void PhysObject::AddForce(const Vector3f& force, CoordSys coordSys)
|
||||
{
|
||||
// http://newtondynamics.com/wiki/index.php5?title=Can_i_dynamicly_move_a_TriMesh%3F
|
||||
NzVector3f min, max;
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys_Global:
|
||||
m_forceAccumulator += force;
|
||||
break;
|
||||
|
||||
case CoordSys_Local:
|
||||
m_forceAccumulator += GetRotation() * force;
|
||||
break;
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void PhysObject::AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys_Global:
|
||||
m_forceAccumulator += force;
|
||||
m_torqueAccumulator += Vector3f::CrossProduct(point - GetMassCenter(CoordSys_Global), force);
|
||||
break;
|
||||
|
||||
case CoordSys_Local:
|
||||
return AddForce(m_matrix.Transform(force, 0.f), m_matrix.Transform(point), CoordSys_Global);
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void PhysObject::AddTorque(const Vector3f& torque, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys_Global:
|
||||
m_torqueAccumulator += torque;
|
||||
break;
|
||||
|
||||
case CoordSys_Local:
|
||||
m_torqueAccumulator += m_matrix.Transform(torque, 0.f);
|
||||
break;
|
||||
}
|
||||
|
||||
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
|
||||
NewtonBodySetSleepState(m_body, 0);
|
||||
}
|
||||
|
||||
void PhysObject::EnableAutoSleep(bool autoSleep)
|
||||
{
|
||||
NewtonBodySetAutoSleep(m_body, autoSleep);
|
||||
}
|
||||
|
||||
Boxf PhysObject::GetAABB() const
|
||||
{
|
||||
Vector3f min, max;
|
||||
NewtonBodyGetAABB(m_body, min, max);
|
||||
|
||||
NewtonWorldForEachBodyInAABBDo(m_world->GetHandle(), min, max, [](const NewtonBody* const body, void* const userData)
|
||||
{
|
||||
NazaraUnused(userData);
|
||||
NewtonBodySetSleepState(body, 0);
|
||||
}, nullptr);
|
||||
return Boxf(min, max);
|
||||
}
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = m_listeners.begin(); it != m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectOnUpdate(this);*/
|
||||
}
|
||||
|
||||
NzPhysObject& NzPhysObject::operator=(NzPhysObject&& object)
|
||||
{
|
||||
if (m_body)
|
||||
NewtonDestroyBody(m_world->GetHandle(), m_body);
|
||||
|
||||
m_body = 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_matrix = std::move(object.m_matrix);
|
||||
m_torqueAccumulator = std::move(object.m_torqueAccumulator);
|
||||
m_world = object.m_world;
|
||||
|
||||
object.m_body = nullptr;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void NzPhysObject::ForceAndTorqueCallback(const NewtonBody* body, float timeStep, int threadIndex)
|
||||
{
|
||||
NazaraUnused(timeStep);
|
||||
NazaraUnused(threadIndex);
|
||||
|
||||
NzPhysObject* me = static_cast<NzPhysObject*>(NewtonBodyGetUserData(body));
|
||||
|
||||
if (!NzNumberEquals(me->m_gravityFactor, 0.f))
|
||||
me->m_forceAccumulator += me->m_world->GetGravity() * me->m_gravityFactor * me->m_mass;
|
||||
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = me->m_listeners.begin(); it != me->m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectApplyForce(me);*/
|
||||
|
||||
NewtonBodySetForce(body, me->m_forceAccumulator);
|
||||
NewtonBodySetTorque(body, me->m_torqueAccumulator);
|
||||
|
||||
me->m_torqueAccumulator.Set(0.f);
|
||||
me->m_forceAccumulator.Set(0.f);
|
||||
|
||||
///TODO: Implanter la force gyroscopique?
|
||||
}
|
||||
|
||||
void NzPhysObject::TransformCallback(const NewtonBody* body, const float* matrix, int threadIndex)
|
||||
{
|
||||
NazaraUnused(threadIndex);
|
||||
|
||||
NzPhysObject* me = static_cast<NzPhysObject*>(NewtonBodyGetUserData(body));
|
||||
me->m_matrix.Set(matrix);
|
||||
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = me->m_listeners.begin(); it != me->m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectOnUpdate(me);*/
|
||||
|
||||
Vector3f PhysObject::GetAngularVelocity() const
|
||||
{
|
||||
Vector3f angularVelocity;
|
||||
NewtonBodyGetOmega(m_body, angularVelocity);
|
||||
|
||||
return angularVelocity;
|
||||
}
|
||||
|
||||
const PhysGeomRef& PhysObject::GetGeom() const
|
||||
{
|
||||
return m_geom;
|
||||
}
|
||||
|
||||
float PhysObject::GetGravityFactor() const
|
||||
{
|
||||
return m_gravityFactor;
|
||||
}
|
||||
|
||||
NewtonBody* PhysObject::GetHandle() const
|
||||
{
|
||||
return m_body;
|
||||
}
|
||||
|
||||
float PhysObject::GetMass() const
|
||||
{
|
||||
return m_mass;
|
||||
}
|
||||
|
||||
Vector3f PhysObject::GetMassCenter(CoordSys coordSys) const
|
||||
{
|
||||
Vector3f center;
|
||||
NewtonBodyGetCentreOfMass(m_body, center);
|
||||
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys_Global:
|
||||
center = m_matrix.Transform(center);
|
||||
break;
|
||||
|
||||
case CoordSys_Local:
|
||||
break; // Aucune opération à effectuer sur le centre de rotation
|
||||
}
|
||||
|
||||
return center;
|
||||
}
|
||||
|
||||
const Matrix4f& PhysObject::GetMatrix() const
|
||||
{
|
||||
return m_matrix;
|
||||
}
|
||||
|
||||
Vector3f PhysObject::GetPosition() const
|
||||
{
|
||||
return m_matrix.GetTranslation();
|
||||
}
|
||||
|
||||
Quaternionf PhysObject::GetRotation() const
|
||||
{
|
||||
return m_matrix.GetRotation();
|
||||
}
|
||||
|
||||
Vector3f PhysObject::GetVelocity() const
|
||||
{
|
||||
Vector3f velocity;
|
||||
NewtonBodyGetVelocity(m_body, velocity);
|
||||
|
||||
return velocity;
|
||||
}
|
||||
|
||||
bool PhysObject::IsAutoSleepEnabled() const
|
||||
{
|
||||
return NewtonBodyGetAutoSleep(m_body) != 0;
|
||||
}
|
||||
|
||||
bool PhysObject::IsMoveable() const
|
||||
{
|
||||
return m_mass > 0.f;
|
||||
}
|
||||
|
||||
bool PhysObject::IsSleeping() const
|
||||
{
|
||||
return NewtonBodyGetSleepState(m_body) != 0;
|
||||
}
|
||||
|
||||
void PhysObject::SetAngularVelocity(const Vector3f& angularVelocity)
|
||||
{
|
||||
NewtonBodySetOmega(m_body, angularVelocity);
|
||||
}
|
||||
|
||||
void PhysObject::SetGeom(PhysGeomRef geom)
|
||||
{
|
||||
if (m_geom.Get() != geom)
|
||||
{
|
||||
if (geom)
|
||||
m_geom = geom;
|
||||
else
|
||||
m_geom = NullGeom::New();
|
||||
|
||||
NewtonBodySetCollision(m_body, m_geom->GetHandle(m_world));
|
||||
}
|
||||
}
|
||||
|
||||
void PhysObject::SetGravityFactor(float gravityFactor)
|
||||
{
|
||||
m_gravityFactor = gravityFactor;
|
||||
}
|
||||
|
||||
void PhysObject::SetMass(float mass)
|
||||
{
|
||||
if (m_mass > 0.f)
|
||||
{
|
||||
float Ix, Iy, Iz;
|
||||
NewtonBodyGetMassMatrix(m_body, &m_mass, &Ix, &Iy, &Iz);
|
||||
float scale = mass/m_mass;
|
||||
NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);
|
||||
}
|
||||
else if (mass > 0.f)
|
||||
{
|
||||
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);
|
||||
NewtonBodySetTransformCallback(m_body, &TransformCallback);
|
||||
}
|
||||
|
||||
m_mass = mass;
|
||||
}
|
||||
|
||||
void PhysObject::SetMassCenter(const Vector3f& center)
|
||||
{
|
||||
if (m_mass > 0.f)
|
||||
NewtonBodySetCentreOfMass(m_body, center);
|
||||
}
|
||||
|
||||
void PhysObject::SetPosition(const Vector3f& position)
|
||||
{
|
||||
m_matrix.SetTranslation(position);
|
||||
UpdateBody();
|
||||
}
|
||||
|
||||
void PhysObject::SetRotation(const Quaternionf& rotation)
|
||||
{
|
||||
m_matrix.SetRotation(rotation);
|
||||
UpdateBody();
|
||||
}
|
||||
|
||||
void PhysObject::SetVelocity(const Vector3f& velocity)
|
||||
{
|
||||
NewtonBodySetVelocity(m_body, velocity);
|
||||
}
|
||||
|
||||
PhysObject& PhysObject::operator=(const PhysObject& object)
|
||||
{
|
||||
PhysObject physObj(object);
|
||||
return operator=(std::move(physObj));
|
||||
}
|
||||
|
||||
void PhysObject::UpdateBody()
|
||||
{
|
||||
NewtonBodySetMatrix(m_body, m_matrix);
|
||||
|
||||
if (NumberEquals(m_mass, 0.f))
|
||||
{
|
||||
// http://newtondynamics.com/wiki/index.php5?title=Can_i_dynamicly_move_a_TriMesh%3F
|
||||
Vector3f min, max;
|
||||
NewtonBodyGetAABB(m_body, min, max);
|
||||
|
||||
NewtonWorldForEachBodyInAABBDo(m_world->GetHandle(), min, max, [](const NewtonBody* const body, void* const userData)
|
||||
{
|
||||
NazaraUnused(userData);
|
||||
NewtonBodySetSleepState(body, 0);
|
||||
}, nullptr);
|
||||
}
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = m_listeners.begin(); it != m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectOnUpdate(this);*/
|
||||
}
|
||||
|
||||
PhysObject& PhysObject::operator=(PhysObject&& object)
|
||||
{
|
||||
if (m_body)
|
||||
NewtonDestroyBody(m_world->GetHandle(), m_body);
|
||||
|
||||
m_body = 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_matrix = std::move(object.m_matrix);
|
||||
m_torqueAccumulator = std::move(object.m_torqueAccumulator);
|
||||
m_world = object.m_world;
|
||||
|
||||
object.m_body = nullptr;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void PhysObject::ForceAndTorqueCallback(const NewtonBody* body, float timeStep, int threadIndex)
|
||||
{
|
||||
NazaraUnused(timeStep);
|
||||
NazaraUnused(threadIndex);
|
||||
|
||||
PhysObject* me = static_cast<PhysObject*>(NewtonBodyGetUserData(body));
|
||||
|
||||
if (!NumberEquals(me->m_gravityFactor, 0.f))
|
||||
me->m_forceAccumulator += me->m_world->GetGravity() * me->m_gravityFactor * me->m_mass;
|
||||
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = me->m_listeners.begin(); it != me->m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectApplyForce(me);*/
|
||||
|
||||
NewtonBodySetForce(body, me->m_forceAccumulator);
|
||||
NewtonBodySetTorque(body, me->m_torqueAccumulator);
|
||||
|
||||
me->m_torqueAccumulator.Set(0.f);
|
||||
me->m_forceAccumulator.Set(0.f);
|
||||
|
||||
///TODO: Implanter la force gyroscopique?
|
||||
}
|
||||
|
||||
void PhysObject::TransformCallback(const NewtonBody* body, const float* matrix, int threadIndex)
|
||||
{
|
||||
NazaraUnused(threadIndex);
|
||||
|
||||
PhysObject* me = static_cast<PhysObject*>(NewtonBodyGetUserData(body));
|
||||
me->m_matrix.Set(matrix);
|
||||
|
||||
/*for (std::set<PhysObjectListener*>::iterator it = me->m_listeners.begin(); it != me->m_listeners.end(); ++it)
|
||||
(*it)->PhysObjectOnUpdate(me);*/
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,57 +6,60 @@
|
||||
#include <Newton/Newton.h>
|
||||
#include <Nazara/Physics/Debug.hpp>
|
||||
|
||||
NzPhysWorld::NzPhysWorld() :
|
||||
m_gravity(NzVector3f::Zero()),
|
||||
m_stepSize(0.005f),
|
||||
m_timestepAccumulator(0.f)
|
||||
namespace Nz
|
||||
{
|
||||
m_world = NewtonCreate();
|
||||
NewtonWorldSetUserData(m_world, this);
|
||||
}
|
||||
|
||||
NzPhysWorld::~NzPhysWorld()
|
||||
{
|
||||
NewtonDestroy(m_world);
|
||||
}
|
||||
|
||||
NzVector3f NzPhysWorld::GetGravity() const
|
||||
{
|
||||
return m_gravity;
|
||||
}
|
||||
|
||||
NewtonWorld* NzPhysWorld::GetHandle() const
|
||||
{
|
||||
return m_world;
|
||||
}
|
||||
|
||||
float NzPhysWorld::GetStepSize() const
|
||||
{
|
||||
return m_stepSize;
|
||||
}
|
||||
|
||||
void NzPhysWorld::SetGravity(const NzVector3f& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
}
|
||||
|
||||
void NzPhysWorld::SetSolverModel(unsigned int model)
|
||||
{
|
||||
NewtonSetSolverModel(m_world, model);
|
||||
}
|
||||
|
||||
void NzPhysWorld::SetStepSize(float stepSize)
|
||||
{
|
||||
m_stepSize = stepSize;
|
||||
}
|
||||
|
||||
void NzPhysWorld::Step(float timestep)
|
||||
{
|
||||
m_timestepAccumulator += timestep;
|
||||
|
||||
while (m_timestepAccumulator >= m_stepSize)
|
||||
PhysWorld::PhysWorld() :
|
||||
m_gravity(Vector3f::Zero()),
|
||||
m_stepSize(0.005f),
|
||||
m_timestepAccumulator(0.f)
|
||||
{
|
||||
NewtonUpdate(m_world, m_stepSize);
|
||||
m_timestepAccumulator -= m_stepSize;
|
||||
m_world = NewtonCreate();
|
||||
NewtonWorldSetUserData(m_world, this);
|
||||
}
|
||||
|
||||
PhysWorld::~PhysWorld()
|
||||
{
|
||||
NewtonDestroy(m_world);
|
||||
}
|
||||
|
||||
Vector3f PhysWorld::GetGravity() const
|
||||
{
|
||||
return m_gravity;
|
||||
}
|
||||
|
||||
NewtonWorld* PhysWorld::GetHandle() const
|
||||
{
|
||||
return m_world;
|
||||
}
|
||||
|
||||
float PhysWorld::GetStepSize() const
|
||||
{
|
||||
return m_stepSize;
|
||||
}
|
||||
|
||||
void PhysWorld::SetGravity(const Vector3f& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
}
|
||||
|
||||
void PhysWorld::SetSolverModel(unsigned int model)
|
||||
{
|
||||
NewtonSetSolverModel(m_world, model);
|
||||
}
|
||||
|
||||
void PhysWorld::SetStepSize(float stepSize)
|
||||
{
|
||||
m_stepSize = stepSize;
|
||||
}
|
||||
|
||||
void PhysWorld::Step(float timestep)
|
||||
{
|
||||
m_timestepAccumulator += timestep;
|
||||
|
||||
while (m_timestepAccumulator >= m_stepSize)
|
||||
{
|
||||
NewtonUpdate(m_world, m_stepSize);
|
||||
m_timestepAccumulator -= m_stepSize;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,57 +10,60 @@
|
||||
#include <Newton/Newton.h>
|
||||
#include <Nazara/Physics/Debug.hpp>
|
||||
|
||||
unsigned int NzPhysics::GetMemoryUsed()
|
||||
namespace Nz
|
||||
{
|
||||
return NewtonGetMemoryUsed();
|
||||
}
|
||||
|
||||
bool NzPhysics::Initialize()
|
||||
{
|
||||
if (s_moduleReferenceCounter > 0)
|
||||
unsigned int Physics::GetMemoryUsed()
|
||||
{
|
||||
return NewtonGetMemoryUsed();
|
||||
}
|
||||
|
||||
bool Physics::Initialize()
|
||||
{
|
||||
if (s_moduleReferenceCounter > 0)
|
||||
{
|
||||
s_moduleReferenceCounter++;
|
||||
return true; // Déjà initialisé
|
||||
}
|
||||
|
||||
// Initialisation des dépendances
|
||||
if (!Core::Initialize())
|
||||
{
|
||||
NazaraError("Failed to initialize core module");
|
||||
return false;
|
||||
}
|
||||
|
||||
s_moduleReferenceCounter++;
|
||||
return true; // Déjà initialisé
|
||||
|
||||
// Initialisation du module
|
||||
|
||||
NazaraNotice("Initialized: Physics module");
|
||||
return true;
|
||||
}
|
||||
|
||||
// Initialisation des dépendances
|
||||
if (!NzCore::Initialize())
|
||||
bool Physics::IsInitialized()
|
||||
{
|
||||
NazaraError("Failed to initialize core module");
|
||||
return false;
|
||||
return s_moduleReferenceCounter != 0;
|
||||
}
|
||||
|
||||
s_moduleReferenceCounter++;
|
||||
|
||||
// Initialisation du module
|
||||
|
||||
NazaraNotice("Initialized: Physics module");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool NzPhysics::IsInitialized()
|
||||
{
|
||||
return s_moduleReferenceCounter != 0;
|
||||
}
|
||||
|
||||
void NzPhysics::Uninitialize()
|
||||
{
|
||||
if (s_moduleReferenceCounter != 1)
|
||||
void Physics::Uninitialize()
|
||||
{
|
||||
// Le module est soit encore utilisé, soit pas initialisé
|
||||
if (s_moduleReferenceCounter > 1)
|
||||
s_moduleReferenceCounter--;
|
||||
if (s_moduleReferenceCounter != 1)
|
||||
{
|
||||
// Le module est soit encore utilisé, soit pas initialisé
|
||||
if (s_moduleReferenceCounter > 1)
|
||||
s_moduleReferenceCounter--;
|
||||
|
||||
return;
|
||||
return;
|
||||
}
|
||||
|
||||
// Libération du module
|
||||
s_moduleReferenceCounter = 0;
|
||||
|
||||
NazaraNotice("Uninitialized: Physics module");
|
||||
|
||||
// Libération des dépendances
|
||||
Core::Uninitialize();
|
||||
}
|
||||
|
||||
// Libération du module
|
||||
s_moduleReferenceCounter = 0;
|
||||
|
||||
NazaraNotice("Uninitialized: Physics module");
|
||||
|
||||
// Libération des dépendances
|
||||
NzCore::Uninitialize();
|
||||
unsigned int Physics::s_moduleReferenceCounter = 0;
|
||||
}
|
||||
|
||||
unsigned int NzPhysics::s_moduleReferenceCounter = 0;
|
||||
|
||||
Reference in New Issue
Block a user