Rename Physics2D to ChipmunkPhysics2D
This commit is contained in:
SirLynix
committed by
Jérôme Leclercq
Jérôme Leclercq
parent
b1255ad2ad
commit
26b23ccce6
102
src/Nazara/ChipmunkPhysics2D/ChipmunkArbiter2D.cpp
Normal file
102
src/Nazara/ChipmunkPhysics2D/ChipmunkArbiter2D.cpp
Normal file
@@ -0,0 +1,102 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkArbiter2D.hpp>
|
||||
#include <chipmunk/chipmunk.h>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
float ChipmunkArbiter2D::ComputeTotalKinematicEnergy() const
|
||||
{
|
||||
return float(cpArbiterTotalKE(m_arbiter));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkArbiter2D::ComputeTotalImpulse() const
|
||||
{
|
||||
cpVect impulse = cpArbiterTotalImpulse(m_arbiter);
|
||||
return Vector2f(Vector2<cpFloat>(impulse.x, impulse.y));
|
||||
}
|
||||
|
||||
std::pair<ChipmunkRigidBody2D*, ChipmunkRigidBody2D*> ChipmunkArbiter2D::GetBodies() const
|
||||
{
|
||||
std::pair<ChipmunkRigidBody2D*, ChipmunkRigidBody2D*> bodies;
|
||||
cpBody* firstBody;
|
||||
cpBody* secondBody;
|
||||
cpArbiterGetBodies(m_arbiter, &firstBody, &secondBody);
|
||||
|
||||
bodies.first = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(firstBody));
|
||||
bodies.second = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(secondBody));
|
||||
|
||||
return bodies;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkArbiter2D::GetContactCount() const
|
||||
{
|
||||
return cpArbiterGetCount(m_arbiter);
|
||||
}
|
||||
|
||||
float ChipmunkArbiter2D::GetContactDepth(std::size_t i) const
|
||||
{
|
||||
return float(cpArbiterGetDepth(m_arbiter, int(i)));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkArbiter2D::GetContactPointA(std::size_t i) const
|
||||
{
|
||||
cpVect point = cpArbiterGetPointA(m_arbiter, int(i));
|
||||
return Vector2f(Vector2<cpFloat>(point.x, point.y));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkArbiter2D::GetContactPointB(std::size_t i) const
|
||||
{
|
||||
cpVect point = cpArbiterGetPointB(m_arbiter, int(i));
|
||||
return Vector2f(Vector2<cpFloat>(point.x, point.y));
|
||||
}
|
||||
|
||||
float ChipmunkArbiter2D::GetElasticity() const
|
||||
{
|
||||
return float(cpArbiterGetRestitution(m_arbiter));
|
||||
}
|
||||
float ChipmunkArbiter2D::GetFriction() const
|
||||
{
|
||||
return float(cpArbiterGetFriction(m_arbiter));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkArbiter2D::GetNormal() const
|
||||
{
|
||||
cpVect normal = cpArbiterGetNormal(m_arbiter);
|
||||
return Vector2f(Vector2<cpFloat>(normal.x, normal.y));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkArbiter2D::GetSurfaceVelocity() const
|
||||
{
|
||||
cpVect velocity = cpArbiterGetNormal(m_arbiter);
|
||||
return Vector2f(Vector2<cpFloat>(velocity.x, velocity.y));
|
||||
}
|
||||
|
||||
bool ChipmunkArbiter2D::IsFirstContact() const
|
||||
{
|
||||
return cpArbiterIsFirstContact(m_arbiter) == cpTrue;
|
||||
}
|
||||
|
||||
bool ChipmunkArbiter2D::IsRemoval() const
|
||||
{
|
||||
return cpArbiterIsRemoval(m_arbiter) == cpTrue;
|
||||
}
|
||||
|
||||
void ChipmunkArbiter2D::SetElasticity(float elasticity)
|
||||
{
|
||||
cpArbiterSetRestitution(m_arbiter, elasticity);
|
||||
}
|
||||
|
||||
void ChipmunkArbiter2D::SetFriction(float friction)
|
||||
{
|
||||
cpArbiterSetFriction(m_arbiter, friction);
|
||||
}
|
||||
|
||||
void ChipmunkArbiter2D::SetSurfaceVelocity(const Vector2f& surfaceVelocity)
|
||||
{
|
||||
cpArbiterSetSurfaceVelocity(m_arbiter, cpv(surfaceVelocity.x, surfaceVelocity.y));
|
||||
}
|
||||
}
|
||||
362
src/Nazara/ChipmunkPhysics2D/ChipmunkCollider2D.cpp
Normal file
362
src/Nazara/ChipmunkPhysics2D/ChipmunkCollider2D.cpp
Normal file
@@ -0,0 +1,362 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkCollider2D.hpp>
|
||||
#include <Nazara/Math/Quaternion.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkPhysWorld2D.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkRigidBody2D.hpp>
|
||||
#include <NazaraUtils/CallOnExit.hpp>
|
||||
#include <NazaraUtils/StackArray.hpp>
|
||||
#include <chipmunk/chipmunk.h>
|
||||
#include <chipmunk/chipmunk_structs.h>
|
||||
#include <array>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
namespace
|
||||
{
|
||||
constexpr cpSpaceDebugColor s_chipmunkWhite = { 1.f, 1.f, 1.f, 1.f };
|
||||
|
||||
Vector2f FromChipmunk(const cpVect& v)
|
||||
{
|
||||
return Vector2f(float(v.x), float(v.y));
|
||||
}
|
||||
}
|
||||
|
||||
ChipmunkCollider2D::~ChipmunkCollider2D() = default;
|
||||
|
||||
void ChipmunkCollider2D::ForEachPolygon(const std::function<void(const Vector2f* vertices, std::size_t vertexCount)>& callback) const
|
||||
{
|
||||
// Currently, the only way to get only the polygons of a shape is to create a temporary cpSpace containing only this shape
|
||||
// A better way to do this would be to reimplement this function in every subclass type in the very same way chipmunk does
|
||||
|
||||
cpSpace* space = cpSpaceNew();
|
||||
if (!space)
|
||||
throw std::runtime_error("failed to create chipmunk space");
|
||||
|
||||
CallOnExit spaceRRID([&] { cpSpaceFree(space); });
|
||||
|
||||
cpBody* body = cpSpaceGetStaticBody(space);
|
||||
|
||||
std::vector<cpShape*> shapes;
|
||||
CreateShapes(body, &shapes);
|
||||
CallOnExit shapeRRID([&]
|
||||
{
|
||||
for (cpShape* shape : shapes)
|
||||
cpShapeDestroy(shape);
|
||||
});
|
||||
|
||||
for (cpShape* shape : shapes)
|
||||
cpSpaceAddShape(space, shape);
|
||||
|
||||
using CallbackType = std::decay_t<decltype(callback)>;
|
||||
|
||||
cpSpaceDebugDrawOptions drawOptions;
|
||||
drawOptions.collisionPointColor = s_chipmunkWhite;
|
||||
drawOptions.constraintColor = s_chipmunkWhite;
|
||||
drawOptions.shapeOutlineColor = s_chipmunkWhite;
|
||||
drawOptions.data = const_cast<void*>(static_cast<const void*>(&callback));
|
||||
drawOptions.flags = CP_SPACE_DEBUG_DRAW_SHAPES;
|
||||
|
||||
// Callback trampoline
|
||||
drawOptions.colorForShape = [](cpShape* /*shape*/, cpDataPointer /*userdata*/) { return s_chipmunkWhite; };
|
||||
drawOptions.drawCircle = [](cpVect pos, cpFloat /*angle*/, cpFloat radius, cpSpaceDebugColor /*outlineColor*/, cpSpaceDebugColor /*fillColor*/, cpDataPointer userdata)
|
||||
{
|
||||
const auto& callback = *static_cast<const CallbackType*>(userdata);
|
||||
|
||||
constexpr std::size_t circleVerticesCount = 20;
|
||||
|
||||
std::array<Vector2f, circleVerticesCount> vertices;
|
||||
|
||||
Vector2f origin = FromChipmunk(pos);
|
||||
float r = SafeCast<float>(radius);
|
||||
|
||||
RadianAnglef angleBetweenVertices = 2.f * Pi<float> / vertices.size();
|
||||
for (std::size_t i = 0; i < vertices.size(); ++i)
|
||||
{
|
||||
RadianAnglef angle = float(i) * angleBetweenVertices;
|
||||
std::pair<float, float> sincos = angle.GetSinCos();
|
||||
|
||||
vertices[i] = origin + Vector2f(r * sincos.first, r * sincos.second);
|
||||
}
|
||||
|
||||
callback(vertices.data(), vertices.size());
|
||||
};
|
||||
|
||||
drawOptions.drawDot = [](cpFloat /*size*/, cpVect /*pos*/, cpSpaceDebugColor /*color*/, cpDataPointer /*userdata*/) {}; //< Dummy
|
||||
|
||||
drawOptions.drawFatSegment = [](cpVect a, cpVect b, cpFloat radius, cpSpaceDebugColor /*outlineColor*/, cpSpaceDebugColor /*fillColor*/, cpDataPointer userdata)
|
||||
{
|
||||
const auto& callback = *static_cast<const CallbackType*>(userdata);
|
||||
|
||||
static std::pair<float, float> sincos = DegreeAnglef(90.f).GetSinCos();
|
||||
|
||||
Vector2f from = FromChipmunk(a);
|
||||
Vector2f to = FromChipmunk(b);
|
||||
|
||||
Vector2f normal = Vector2f::Normalize(to - from);
|
||||
Vector2f thicknessNormal(sincos.second * normal.x - sincos.first * normal.y,
|
||||
sincos.first * normal.x + sincos.second * normal.y);
|
||||
|
||||
float thickness = SafeCast<float>(radius);
|
||||
|
||||
std::array<Vector2f, 4> vertices;
|
||||
vertices[0] = from + thickness * thicknessNormal;
|
||||
vertices[1] = from - thickness * thicknessNormal;
|
||||
vertices[2] = to - thickness * thicknessNormal;
|
||||
vertices[3] = to + thickness * thicknessNormal;
|
||||
|
||||
callback(vertices.data(), vertices.size());
|
||||
};
|
||||
|
||||
drawOptions.drawPolygon = [](int vertexCount, const cpVect* vertices, cpFloat /*radius*/, cpSpaceDebugColor /*outlineColor*/, cpSpaceDebugColor /*fillColor*/, cpDataPointer userdata)
|
||||
{
|
||||
const auto& callback = *static_cast<const CallbackType*>(userdata);
|
||||
|
||||
StackArray<Vector2f> nVertices = NazaraStackArray(Vector2f, vertexCount);
|
||||
for (int i = 0; i < vertexCount; ++i)
|
||||
nVertices[i].Set(float(vertices[i].x), float(vertices[i].y));
|
||||
|
||||
callback(nVertices.data(), nVertices.size());
|
||||
};
|
||||
|
||||
drawOptions.drawSegment = [](cpVect a, cpVect b, cpSpaceDebugColor /*fillColor*/, cpDataPointer userdata)
|
||||
{
|
||||
const auto& callback = *static_cast<const CallbackType*>(userdata);
|
||||
|
||||
std::array<Vector2f, 2> vertices = { FromChipmunk(a), FromChipmunk(b) };
|
||||
callback(vertices.data(), vertices.size());
|
||||
};
|
||||
|
||||
cpSpaceDebugDraw(space, &drawOptions);
|
||||
}
|
||||
|
||||
std::size_t ChipmunkCollider2D::GenerateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
std::size_t shapeCount = CreateShapes(body, shapes);
|
||||
|
||||
cpShapeFilter filter = cpShapeFilterNew(m_collisionGroup, m_categoryMask, m_collisionMask);
|
||||
for (std::size_t i = shapes->size() - shapeCount; i < shapes->size(); ++i)
|
||||
{
|
||||
cpShape* shape = (*shapes)[i];
|
||||
|
||||
cpShapeSetCollisionType(shape, m_collisionId);
|
||||
cpShapeSetElasticity(shape, cpFloat(m_elasticity));
|
||||
cpShapeSetFilter(shape, filter);
|
||||
cpShapeSetFriction(shape, cpFloat(m_friction));
|
||||
cpShapeSetSensor(shape, (m_trigger) ? cpTrue : cpFalse);
|
||||
cpShapeSetSurfaceVelocity(shape, cpv(cpFloat(m_surfaceVelocity.x), cpFloat(m_surfaceVelocity.y)));
|
||||
}
|
||||
|
||||
return shapeCount;
|
||||
}
|
||||
|
||||
/******************************** BoxCollider2D *********************************/
|
||||
|
||||
ChipmunkBoxCollider2D::ChipmunkBoxCollider2D(const Vector2f& size, float radius) :
|
||||
ChipmunkBoxCollider2D(Rectf(-size.x / 2.f, -size.y / 2.f, size.x, size.y), radius)
|
||||
{
|
||||
}
|
||||
|
||||
ChipmunkBoxCollider2D::ChipmunkBoxCollider2D(const Rectf& rect, float radius) :
|
||||
m_rect(rect),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
Vector2f ChipmunkBoxCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
return m_rect.GetCenter();
|
||||
}
|
||||
|
||||
float ChipmunkBoxCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
return SafeCast<float>(cpMomentForBox2(mass, cpBBNew(m_rect.x, m_rect.y, m_rect.x + m_rect.width, m_rect.y + m_rect.height)));
|
||||
}
|
||||
|
||||
ChipmunkColliderType2D ChipmunkBoxCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Box;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkBoxCollider2D::CreateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
shapes->push_back(cpBoxShapeNew2(body, cpBBNew(m_rect.x, m_rect.y, m_rect.x + m_rect.width, m_rect.y + m_rect.height), m_radius));
|
||||
return 1;
|
||||
}
|
||||
|
||||
/******************************** CircleCollider2D *********************************/
|
||||
|
||||
ChipmunkCircleCollider2D::ChipmunkCircleCollider2D(float radius, const Vector2f& offset) :
|
||||
m_offset(offset),
|
||||
m_radius(radius)
|
||||
{
|
||||
}
|
||||
|
||||
Vector2f ChipmunkCircleCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
return m_offset;
|
||||
}
|
||||
|
||||
float ChipmunkCircleCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
return SafeCast<float>(cpMomentForCircle(mass, 0.f, m_radius, cpv(m_offset.x, m_offset.y)));
|
||||
}
|
||||
|
||||
ChipmunkColliderType2D ChipmunkCircleCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Circle;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkCircleCollider2D::CreateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
shapes->push_back(cpCircleShapeNew(body, m_radius, cpv(m_offset.x, m_offset.y)));
|
||||
return 1;
|
||||
}
|
||||
|
||||
/******************************** CompoundCollider2D *********************************/
|
||||
|
||||
ChipmunkCompoundCollider2D::ChipmunkCompoundCollider2D(std::vector<std::shared_ptr<ChipmunkCollider2D>> geoms) :
|
||||
m_geoms(std::move(geoms)),
|
||||
m_doesOverrideCollisionProperties(true)
|
||||
{
|
||||
}
|
||||
|
||||
Vector2f ChipmunkCompoundCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
Vector2f centerOfMass = Vector2f::Zero();
|
||||
for (const auto& geom : m_geoms)
|
||||
centerOfMass += geom->ComputeCenterOfMass();
|
||||
|
||||
return centerOfMass / float(m_geoms.size());
|
||||
}
|
||||
|
||||
float ChipmunkCompoundCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
///TODO: Correctly compute moment using parallel axis theorem:
|
||||
/// https://chipmunk-physics.net/forum/viewtopic.php?t=1056
|
||||
float momentOfInertia = 0.f;
|
||||
for (const auto& geom : m_geoms)
|
||||
momentOfInertia += geom->ComputeMomentOfInertia(mass); //< Eeeer
|
||||
|
||||
return momentOfInertia;
|
||||
}
|
||||
|
||||
ChipmunkColliderType2D ChipmunkCompoundCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Compound;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkCompoundCollider2D::CreateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
// Since C++ does not allow protected call from other objects, we have to be a friend of Collider2D, yay
|
||||
|
||||
std::size_t shapeCount = 0;
|
||||
for (const auto& geom : m_geoms)
|
||||
shapeCount += geom->CreateShapes(body, shapes);
|
||||
|
||||
return shapeCount;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkCompoundCollider2D::GenerateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
// This is our parent's default behavior
|
||||
if (m_doesOverrideCollisionProperties)
|
||||
return ChipmunkCollider2D::GenerateShapes(body, shapes);
|
||||
else
|
||||
{
|
||||
std::size_t shapeCount = 0;
|
||||
for (const auto& geom : m_geoms)
|
||||
shapeCount += geom->GenerateShapes(body, shapes);
|
||||
|
||||
return shapeCount;
|
||||
}
|
||||
}
|
||||
|
||||
/******************************** ConvexCollider2D *********************************/
|
||||
|
||||
ChipmunkConvexCollider2D::ChipmunkConvexCollider2D(SparsePtr<const Vector2f> vertices, std::size_t vertexCount, float radius) :
|
||||
m_radius(radius)
|
||||
{
|
||||
m_vertices.resize(vertexCount);
|
||||
for (std::size_t i = 0; i < vertexCount; ++i)
|
||||
m_vertices[i].Set(*vertices++);
|
||||
}
|
||||
|
||||
Vector2f ChipmunkConvexCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
static_assert(sizeof(cpVect) == sizeof(Vector2d), "Chipmunk vector is not equivalent to Vector2d");
|
||||
|
||||
cpVect center = cpCentroidForPoly(int(m_vertices.size()), reinterpret_cast<const cpVect*>(m_vertices.data()));
|
||||
|
||||
return Vector2f(float(center.x), float(center.y));
|
||||
}
|
||||
|
||||
float ChipmunkConvexCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
static_assert(sizeof(cpVect) == sizeof(Vector2d), "Chipmunk vector is not equivalent to Vector2d");
|
||||
|
||||
return SafeCast<float>(cpMomentForPoly(mass, int(m_vertices.size()), reinterpret_cast<const cpVect*>(m_vertices.data()), cpv(0.0, 0.0), m_radius));
|
||||
}
|
||||
|
||||
ChipmunkColliderType2D ChipmunkConvexCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Convex;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkConvexCollider2D::CreateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
shapes->push_back(cpPolyShapeNew(body, int(m_vertices.size()), reinterpret_cast<const cpVect*>(m_vertices.data()), cpTransformIdentity, m_radius));
|
||||
return 1;
|
||||
}
|
||||
|
||||
/********************************* NullCollider2D **********************************/
|
||||
|
||||
ChipmunkColliderType2D ChipmunkNullCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Null;
|
||||
}
|
||||
|
||||
Vector2f ChipmunkNullCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
return Vector2f::Zero();
|
||||
}
|
||||
|
||||
float ChipmunkNullCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
return (mass > 0.f) ? 1.f : 0.f; //< Null inertia is only possible for static/kinematic objects
|
||||
}
|
||||
|
||||
std::size_t ChipmunkNullCollider2D::CreateShapes(cpBody* /*body*/, std::vector<cpShape*>* /*shapes*/) const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/******************************** SegmentCollider2D *********************************/
|
||||
|
||||
Vector2f ChipmunkSegmentCollider2D::ComputeCenterOfMass() const
|
||||
{
|
||||
return (m_first + m_second) / 2.f;
|
||||
}
|
||||
|
||||
float ChipmunkSegmentCollider2D::ComputeMomentOfInertia(float mass) const
|
||||
{
|
||||
return SafeCast<float>(cpMomentForSegment(mass, cpv(m_first.x, m_first.y), cpv(m_second.x, m_second.y), m_thickness));
|
||||
}
|
||||
|
||||
ChipmunkColliderType2D ChipmunkSegmentCollider2D::GetType() const
|
||||
{
|
||||
return ChipmunkColliderType2D::Segment;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkSegmentCollider2D::CreateShapes(cpBody* body, std::vector<cpShape*>* shapes) const
|
||||
{
|
||||
cpShape* segment = cpSegmentShapeNew(body, cpv(m_first.x, m_first.y), cpv(m_second.x, m_second.y), m_thickness);
|
||||
cpSegmentShapeSetNeighbors(segment, cpv(m_firstNeighbor.x, m_firstNeighbor.y), cpv(m_secondNeighbor.x, m_secondNeighbor.y));
|
||||
|
||||
shapes->push_back(segment);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
428
src/Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.cpp
Normal file
428
src/Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.cpp
Normal file
@@ -0,0 +1,428 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.hpp>
|
||||
#include <chipmunk/chipmunk.h>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
ChipmunkConstraint2D::ChipmunkConstraint2D(ChipmunkPhysWorld2D* world, cpConstraint* constraint) :
|
||||
m_constraint(constraint)
|
||||
{
|
||||
cpConstraintSetUserData(m_constraint, this);
|
||||
cpSpaceAddConstraint(world->GetHandle(), m_constraint);
|
||||
}
|
||||
|
||||
ChipmunkConstraint2D::ChipmunkConstraint2D(ChipmunkConstraint2D&& rhs) :
|
||||
m_constraint(std::move(rhs.m_constraint))
|
||||
{
|
||||
cpConstraintSetUserData(m_constraint, this);
|
||||
}
|
||||
|
||||
ChipmunkConstraint2D::~ChipmunkConstraint2D()
|
||||
{
|
||||
cpSpaceRemoveConstraint(cpConstraintGetSpace(m_constraint), m_constraint);
|
||||
}
|
||||
|
||||
void ChipmunkConstraint2D::EnableBodyCollision(bool enable)
|
||||
{
|
||||
cpConstraintSetCollideBodies(m_constraint, (enable) ? cpTrue : cpFalse);
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyA()
|
||||
{
|
||||
return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyA(m_constraint)));
|
||||
}
|
||||
|
||||
const ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyA() const
|
||||
{
|
||||
return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyA(m_constraint)));
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyB()
|
||||
{
|
||||
return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyB(m_constraint)));
|
||||
}
|
||||
|
||||
const ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyB() const
|
||||
{
|
||||
return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyB(m_constraint)));
|
||||
}
|
||||
|
||||
float ChipmunkConstraint2D::GetErrorBias() const
|
||||
{
|
||||
return float(cpConstraintGetErrorBias(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkConstraint2D::GetLastImpulse() const
|
||||
{
|
||||
return float(cpConstraintGetImpulse(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkConstraint2D::GetMaxBias() const
|
||||
{
|
||||
return float(cpConstraintGetMaxBias(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkConstraint2D::GetMaxForce() const
|
||||
{
|
||||
return float(cpConstraintGetMaxForce(m_constraint));
|
||||
}
|
||||
|
||||
ChipmunkPhysWorld2D& ChipmunkConstraint2D::GetWorld()
|
||||
{
|
||||
return *static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(cpConstraintGetSpace(m_constraint)));
|
||||
}
|
||||
|
||||
const ChipmunkPhysWorld2D& ChipmunkConstraint2D::GetWorld() const
|
||||
{
|
||||
return *static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(cpConstraintGetSpace(m_constraint)));
|
||||
}
|
||||
|
||||
bool ChipmunkConstraint2D::IsBodyCollisionEnabled() const
|
||||
{
|
||||
return cpConstraintGetCollideBodies(m_constraint) == cpTrue;
|
||||
}
|
||||
|
||||
void ChipmunkConstraint2D::SetErrorBias(float bias)
|
||||
{
|
||||
cpConstraintSetErrorBias(m_constraint, bias);
|
||||
}
|
||||
|
||||
void ChipmunkConstraint2D::SetMaxBias(float bias)
|
||||
{
|
||||
cpConstraintSetMaxBias(m_constraint, bias);
|
||||
}
|
||||
|
||||
void ChipmunkConstraint2D::SetMaxForce(float force)
|
||||
{
|
||||
cpConstraintSetMaxForce(m_constraint, force);
|
||||
}
|
||||
|
||||
ChipmunkConstraint2D& ChipmunkConstraint2D::operator=(ChipmunkConstraint2D && rhs)
|
||||
{
|
||||
m_constraint = std::move(rhs.m_constraint);
|
||||
cpConstraintSetUserData(m_constraint, this);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
ChipmunkDampedSpringConstraint2D::ChipmunkDampedSpringConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor, float restLength, float stiffness, float damping) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpDampedSpringNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y), restLength, stiffness, damping))
|
||||
{
|
||||
}
|
||||
|
||||
float ChipmunkDampedSpringConstraint2D::GetDamping() const
|
||||
{
|
||||
return float(cpDampedSpringGetDamping(m_constraint));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkDampedSpringConstraint2D::GetFirstAnchor() const
|
||||
{
|
||||
cpVect anchor = cpDampedSpringGetAnchorA(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
float ChipmunkDampedSpringConstraint2D::GetRestLength() const
|
||||
{
|
||||
return float(cpDampedSpringGetRestLength(m_constraint));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkDampedSpringConstraint2D::GetSecondAnchor() const
|
||||
{
|
||||
cpVect anchor = cpDampedSpringGetAnchorB(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
float ChipmunkDampedSpringConstraint2D::GetStiffness() const
|
||||
{
|
||||
return float(cpDampedSpringGetStiffness(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkDampedSpringConstraint2D::SetDamping(float newDamping)
|
||||
{
|
||||
cpDampedSpringSetDamping(m_constraint, newDamping);
|
||||
}
|
||||
|
||||
void ChipmunkDampedSpringConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpDampedSpringSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkDampedSpringConstraint2D::SetRestLength(float newLength)
|
||||
{
|
||||
cpDampedSpringSetRestLength(m_constraint, newLength);
|
||||
}
|
||||
|
||||
void ChipmunkDampedSpringConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpDampedSpringSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkDampedSpringConstraint2D::SetStiffness(float newStiffness)
|
||||
{
|
||||
cpDampedSpringSetStiffness(m_constraint, newStiffness);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkDampedRotarySpringConstraint2D::ChipmunkDampedRotarySpringConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const RadianAnglef& restAngle, float stiffness, float damping) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpDampedRotarySpringNew(first.GetHandle(), second.GetHandle(), restAngle.value, stiffness, damping))
|
||||
{
|
||||
}
|
||||
|
||||
float ChipmunkDampedRotarySpringConstraint2D::GetDamping() const
|
||||
{
|
||||
return float(cpDampedRotarySpringGetDamping(m_constraint));
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkDampedRotarySpringConstraint2D::GetRestAngle() const
|
||||
{
|
||||
return float(cpDampedRotarySpringGetRestAngle(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkDampedRotarySpringConstraint2D::GetStiffness() const
|
||||
{
|
||||
return float(cpDampedRotarySpringGetStiffness(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkDampedRotarySpringConstraint2D::SetDamping(float newDamping)
|
||||
{
|
||||
cpDampedSpringSetDamping(m_constraint, newDamping);
|
||||
}
|
||||
|
||||
void ChipmunkDampedRotarySpringConstraint2D::SetRestAngle(const RadianAnglef& newAngle)
|
||||
{
|
||||
cpDampedRotarySpringSetRestAngle(m_constraint, newAngle.value);
|
||||
}
|
||||
|
||||
void ChipmunkDampedRotarySpringConstraint2D::SetStiffness(float newStiffness)
|
||||
{
|
||||
cpDampedRotarySpringSetStiffness(m_constraint, newStiffness);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkGearConstraint2D::ChipmunkGearConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, float phase, float ratio) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpGearJointNew(first.GetHandle(), second.GetHandle(), phase, ratio))
|
||||
{
|
||||
}
|
||||
|
||||
float ChipmunkGearConstraint2D::GetPhase() const
|
||||
{
|
||||
return float(cpGearJointGetPhase(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkGearConstraint2D::GetRatio() const
|
||||
{
|
||||
return float(cpGearJointGetRatio(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkGearConstraint2D::SetPhase(float phase)
|
||||
{
|
||||
cpGearJointSetPhase(m_constraint, phase);
|
||||
}
|
||||
|
||||
void ChipmunkGearConstraint2D::SetRatio(float ratio)
|
||||
{
|
||||
cpGearJointSetRatio(m_constraint, ratio);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkMotorConstraint2D::ChipmunkMotorConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, float rate) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpSimpleMotorNew(first.GetHandle(), second.GetHandle(), rate))
|
||||
{
|
||||
}
|
||||
|
||||
float ChipmunkMotorConstraint2D::GetRate() const
|
||||
{
|
||||
return float(cpSimpleMotorGetRate(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkMotorConstraint2D::SetRate(float rate)
|
||||
{
|
||||
cpSimpleMotorSetRate(m_constraint, rate);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkPinConstraint2D::ChipmunkPinConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpPinJointNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y)))
|
||||
{
|
||||
}
|
||||
|
||||
float ChipmunkPinConstraint2D::GetDistance() const
|
||||
{
|
||||
return float(cpPinJointGetDist(m_constraint));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkPinConstraint2D::GetFirstAnchor() const
|
||||
{
|
||||
cpVect anchor = cpPinJointGetAnchorA(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkPinConstraint2D::GetSecondAnchor() const
|
||||
{
|
||||
cpVect anchor = cpPinJointGetAnchorB(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkPinConstraint2D::SetDistance(float newDistance)
|
||||
{
|
||||
cpPinJointSetDist(m_constraint, newDistance);
|
||||
}
|
||||
|
||||
void ChipmunkPinConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpPinJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkPinConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpPinJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
|
||||
ChipmunkPivotConstraint2D::ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& anchor) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew(first.GetHandle(), second.GetHandle(), cpv(anchor.x, anchor.y)))
|
||||
{
|
||||
}
|
||||
|
||||
ChipmunkPivotConstraint2D::ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew2(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y)))
|
||||
{
|
||||
}
|
||||
|
||||
Vector2f ChipmunkPivotConstraint2D::GetFirstAnchor() const
|
||||
{
|
||||
cpVect anchor = cpPivotJointGetAnchorA(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkPivotConstraint2D::GetSecondAnchor() const
|
||||
{
|
||||
cpVect anchor = cpPivotJointGetAnchorB(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkPivotConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpPivotJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkPivotConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpPivotJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
|
||||
ChipmunkRatchetConstraint2D::ChipmunkRatchetConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, float phase, float ratchet) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpRatchetJointNew(first.GetHandle(), second.GetHandle(), phase, ratchet))
|
||||
{
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkRatchetConstraint2D::GetAngle() const
|
||||
{
|
||||
return float(cpRatchetJointGetAngle(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkRatchetConstraint2D::GetPhase() const
|
||||
{
|
||||
return float(cpRatchetJointGetPhase(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkRatchetConstraint2D::GetRatchet() const
|
||||
{
|
||||
return float(cpRatchetJointGetRatchet(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkRatchetConstraint2D::SetAngle(const RadianAnglef& angle)
|
||||
{
|
||||
cpRatchetJointSetAngle(m_constraint, angle.value);
|
||||
}
|
||||
|
||||
void ChipmunkRatchetConstraint2D::SetPhase(float phase)
|
||||
{
|
||||
cpRatchetJointSetPhase(m_constraint, phase);
|
||||
}
|
||||
|
||||
void ChipmunkRatchetConstraint2D::SetRatchet(float ratchet)
|
||||
{
|
||||
cpRatchetJointSetRatchet(m_constraint, ratchet);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkRotaryLimitConstraint2D::ChipmunkRotaryLimitConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const RadianAnglef& minAngle, const RadianAnglef& maxAngle) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpRotaryLimitJointNew(first.GetHandle(), second.GetHandle(), minAngle.value, maxAngle.value))
|
||||
{
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkRotaryLimitConstraint2D::GetMaxAngle() const
|
||||
{
|
||||
return float(cpRotaryLimitJointGetMax(m_constraint));
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkRotaryLimitConstraint2D::GetMinAngle() const
|
||||
{
|
||||
return float(cpRotaryLimitJointGetMax(m_constraint));
|
||||
}
|
||||
|
||||
void ChipmunkRotaryLimitConstraint2D::SetMaxAngle(const RadianAnglef& maxAngle)
|
||||
{
|
||||
cpRotaryLimitJointSetMax(m_constraint, maxAngle.value);
|
||||
}
|
||||
|
||||
void ChipmunkRotaryLimitConstraint2D::SetMinAngle(const RadianAnglef& minAngle)
|
||||
{
|
||||
cpRotaryLimitJointSetMin(m_constraint, minAngle.value);
|
||||
}
|
||||
|
||||
|
||||
ChipmunkSlideConstraint2D::ChipmunkSlideConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor, float min, float max) :
|
||||
ChipmunkConstraint2D(first.GetWorld(), cpSlideJointNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y), min, max))
|
||||
{
|
||||
}
|
||||
|
||||
Vector2f ChipmunkSlideConstraint2D::GetFirstAnchor() const
|
||||
{
|
||||
cpVect anchor = cpSlideJointGetAnchorA(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
float ChipmunkSlideConstraint2D::GetMaxDistance() const
|
||||
{
|
||||
return float(cpSlideJointGetMax(m_constraint));
|
||||
}
|
||||
|
||||
float ChipmunkSlideConstraint2D::GetMinDistance() const
|
||||
{
|
||||
return float(cpSlideJointGetMin(m_constraint));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkSlideConstraint2D::GetSecondAnchor() const
|
||||
{
|
||||
cpVect anchor = cpSlideJointGetAnchorB(m_constraint);
|
||||
return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkSlideConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpSlideJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
|
||||
void ChipmunkSlideConstraint2D::SetMaxDistance(float newMaxDistance)
|
||||
{
|
||||
cpSlideJointSetMax(m_constraint, newMaxDistance);
|
||||
}
|
||||
|
||||
void ChipmunkSlideConstraint2D::SetMinDistance(float newMinDistance)
|
||||
{
|
||||
cpSlideJointSetMin(m_constraint, newMinDistance);
|
||||
}
|
||||
|
||||
void ChipmunkSlideConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
|
||||
{
|
||||
cpSlideJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
|
||||
}
|
||||
}
|
||||
543
src/Nazara/ChipmunkPhysics2D/ChipmunkPhysWorld2D.cpp
Normal file
543
src/Nazara/ChipmunkPhysics2D/ChipmunkPhysWorld2D.cpp
Normal file
@@ -0,0 +1,543 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkPhysWorld2D.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkArbiter2D.hpp>
|
||||
#include <NazaraUtils/StackArray.hpp>
|
||||
#include <chipmunk/chipmunk.h>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
namespace
|
||||
{
|
||||
Color CpDebugColorToColor(cpSpaceDebugColor c)
|
||||
{
|
||||
return Color{ c.r, c.g, c.b, c.a };
|
||||
}
|
||||
|
||||
cpSpaceDebugColor ColorToCpDebugColor(Color c)
|
||||
{
|
||||
return cpSpaceDebugColor{ c.r, c.g, c.b, c.a };
|
||||
}
|
||||
|
||||
void CpCircleCallback(cpVect pos, cpFloat angle, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->circleCallback)
|
||||
drawOptions->circleCallback(Vector2f(float(pos.x), float(pos.y)), RadianAnglef(float(angle)), float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata);
|
||||
}
|
||||
|
||||
void CpDotCallback(cpFloat size, cpVect pos, cpSpaceDebugColor color, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->dotCallback)
|
||||
drawOptions->dotCallback(Vector2f(float(pos.x), float(pos.y)), float(size), CpDebugColorToColor(color), drawOptions->userdata);
|
||||
}
|
||||
|
||||
void CpPolygonCallback(int vertexCount, const cpVect* vertices, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->polygonCallback)
|
||||
{
|
||||
//TODO: constexpr if to prevent copy/cast if sizeof(cpVect) == sizeof(Vector2f)
|
||||
|
||||
StackArray<Vector2f> nVertices = NazaraStackArray(Vector2f, vertexCount);
|
||||
for (int i = 0; i < vertexCount; ++i)
|
||||
nVertices[i].Set(float(vertices[i].x), float(vertices[i].y));
|
||||
|
||||
drawOptions->polygonCallback(nVertices.data(), vertexCount, float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata);
|
||||
}
|
||||
}
|
||||
|
||||
void CpSegmentCallback(cpVect a, cpVect b, cpSpaceDebugColor color, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->segmentCallback)
|
||||
drawOptions->segmentCallback(Vector2f(float(a.x), float(a.y)), Vector2f(float(b.x), float(b.y)), CpDebugColorToColor(color), drawOptions->userdata);
|
||||
}
|
||||
|
||||
void CpThickSegmentCallback(cpVect a, cpVect b, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->thickSegmentCallback)
|
||||
drawOptions->thickSegmentCallback(Vector2f(float(a.x), float(a.y)), Vector2f(float(b.x), float(b.y)), float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata);
|
||||
}
|
||||
|
||||
cpSpaceDebugColor CpShapeColorCallback(cpShape* shape, cpDataPointer userdata)
|
||||
{
|
||||
auto drawOptions = static_cast<ChipmunkPhysWorld2D::DebugDrawOptions*>(userdata);
|
||||
if (drawOptions->colorCallback)
|
||||
{
|
||||
ChipmunkRigidBody2D& rigidBody = *static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape));
|
||||
return ColorToCpDebugColor(drawOptions->colorCallback(rigidBody, rigidBody.GetShapeIndex(shape), drawOptions->userdata));
|
||||
}
|
||||
else
|
||||
return cpSpaceDebugColor{1.f, 0.f, 0.f, 1.f};
|
||||
}
|
||||
}
|
||||
|
||||
ChipmunkPhysWorld2D::ChipmunkPhysWorld2D() :
|
||||
m_maxStepCount(50),
|
||||
m_stepSize(Time::TickDuration(200)),
|
||||
m_timestepAccumulator(Time::Zero())
|
||||
{
|
||||
m_handle = cpSpaceNew();
|
||||
cpSpaceSetUserData(m_handle, this);
|
||||
}
|
||||
|
||||
ChipmunkPhysWorld2D::~ChipmunkPhysWorld2D()
|
||||
{
|
||||
cpSpaceFree(m_handle);
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::DebugDraw(const DebugDrawOptions& options, bool drawShapes, bool drawConstraints, bool drawCollisions)
|
||||
{
|
||||
auto ColorToCpDebugColor = [](Color c) -> cpSpaceDebugColor
|
||||
{
|
||||
return cpSpaceDebugColor{ c.r, c.g, c.b, c.a };
|
||||
};
|
||||
|
||||
cpSpaceDebugDrawOptions drawOptions;
|
||||
drawOptions.collisionPointColor = ColorToCpDebugColor(options.collisionPointColor);
|
||||
drawOptions.constraintColor = ColorToCpDebugColor(options.constraintColor);
|
||||
drawOptions.shapeOutlineColor = ColorToCpDebugColor(options.shapeOutlineColor);
|
||||
drawOptions.data = const_cast<DebugDrawOptions*>(&options); //< Yeah, I know, shame :bell: but it won't be used for writing anyway
|
||||
|
||||
std::underlying_type_t<cpSpaceDebugDrawFlags> drawFlags = 0;
|
||||
if (drawCollisions)
|
||||
drawFlags |= CP_SPACE_DEBUG_DRAW_COLLISION_POINTS;
|
||||
|
||||
if (drawConstraints)
|
||||
drawFlags |= CP_SPACE_DEBUG_DRAW_CONSTRAINTS;
|
||||
|
||||
if (drawShapes)
|
||||
drawFlags |= CP_SPACE_DEBUG_DRAW_SHAPES;
|
||||
|
||||
drawOptions.flags = static_cast<cpSpaceDebugDrawFlags>(drawFlags);
|
||||
|
||||
// Callback trampoline
|
||||
drawOptions.colorForShape = CpShapeColorCallback;
|
||||
drawOptions.drawCircle = CpCircleCallback;
|
||||
drawOptions.drawDot = CpDotCallback;
|
||||
drawOptions.drawFatSegment = CpThickSegmentCallback;
|
||||
drawOptions.drawPolygon = CpPolygonCallback;
|
||||
drawOptions.drawSegment = CpSegmentCallback;
|
||||
|
||||
cpSpaceDebugDraw(m_handle, &drawOptions);
|
||||
}
|
||||
|
||||
float ChipmunkPhysWorld2D::GetDamping() const
|
||||
{
|
||||
return float(cpSpaceGetDamping(m_handle));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkPhysWorld2D::GetGravity() const
|
||||
{
|
||||
cpVect gravity = cpSpaceGetGravity(m_handle);
|
||||
return Vector2f(Vector2<cpFloat>(gravity.x, gravity.y));
|
||||
}
|
||||
|
||||
cpSpace* ChipmunkPhysWorld2D::GetHandle() const
|
||||
{
|
||||
return m_handle;
|
||||
}
|
||||
|
||||
std::size_t ChipmunkPhysWorld2D::GetIterationCount() const
|
||||
{
|
||||
return cpSpaceGetIterations(m_handle);
|
||||
}
|
||||
|
||||
std::size_t ChipmunkPhysWorld2D::GetMaxStepCount() const
|
||||
{
|
||||
return m_maxStepCount;
|
||||
}
|
||||
|
||||
Time ChipmunkPhysWorld2D::GetStepSize() const
|
||||
{
|
||||
return m_stepSize;
|
||||
}
|
||||
|
||||
bool ChipmunkPhysWorld2D::NearestBodyQuery(const Vector2f & from, float maxDistance, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, ChipmunkRigidBody2D** nearestBody)
|
||||
{
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
|
||||
if (cpShape* shape = cpSpacePointQueryNearest(m_handle, {from.x, from.y}, maxDistance, filter, nullptr))
|
||||
{
|
||||
if (nearestBody)
|
||||
*nearestBody = static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape));
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ChipmunkPhysWorld2D::NearestBodyQuery(const Vector2f& from, float maxDistance, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, NearestQueryResult* result)
|
||||
{
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
|
||||
if (result)
|
||||
{
|
||||
cpPointQueryInfo queryInfo;
|
||||
|
||||
if (cpSpacePointQueryNearest(m_handle, { from.x, from.y }, maxDistance, filter, &queryInfo))
|
||||
{
|
||||
result->closestPoint.Set(Vector2<cpFloat>(queryInfo.point.x, queryInfo.point.y));
|
||||
result->distance = float(queryInfo.distance);
|
||||
result->fraction.Set(Vector2<cpFloat>(queryInfo.gradient.x, queryInfo.gradient.y));
|
||||
result->nearestBody = static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(queryInfo.shape));
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cpSpacePointQueryNearest(m_handle, { from.x, from.y }, maxDistance, filter, nullptr))
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RaycastQuery(const Vector2f& from, const Vector2f& to, float radius, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, const FunctionRef<void(const RaycastHit&)>& callback)
|
||||
{
|
||||
using CallbackType = std::remove_reference_t<decltype(callback)>;
|
||||
|
||||
auto cpCallback = [](cpShape* shape, cpVect point, cpVect normal, cpFloat alpha, void* data)
|
||||
{
|
||||
CallbackType& callback = *static_cast<CallbackType*>(data);
|
||||
|
||||
RaycastHit hitInfo;
|
||||
hitInfo.fraction = float(alpha);
|
||||
hitInfo.hitNormal.Set(Vector2<cpFloat>(normal.x, normal.y));
|
||||
hitInfo.hitPos.Set(Vector2<cpFloat>(point.x, point.y));
|
||||
hitInfo.nearestBody = static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape));
|
||||
|
||||
callback(hitInfo);
|
||||
};
|
||||
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
cpSpaceSegmentQuery(m_handle, { from.x, from.y }, { to.x, to.y }, radius, filter, cpCallback, const_cast<void*>(static_cast<const void*>(&callback)));
|
||||
}
|
||||
|
||||
bool ChipmunkPhysWorld2D::RaycastQuery(const Vector2f& from, const Vector2f& to, float radius, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, std::vector<RaycastHit>* hitInfos)
|
||||
{
|
||||
using ResultType = decltype(hitInfos);
|
||||
|
||||
auto callback = [](cpShape* shape, cpVect point, cpVect normal, cpFloat alpha, void* data)
|
||||
{
|
||||
ResultType results = static_cast<ResultType>(data);
|
||||
|
||||
RaycastHit hitInfo;
|
||||
hitInfo.fraction = float(alpha);
|
||||
hitInfo.hitNormal.Set(Vector2<cpFloat>(normal.x, normal.y));
|
||||
hitInfo.hitPos.Set(Vector2<cpFloat>(point.x, point.y));
|
||||
hitInfo.nearestBody = static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape));
|
||||
|
||||
results->emplace_back(std::move(hitInfo));
|
||||
};
|
||||
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
|
||||
std::size_t previousSize = hitInfos->size();
|
||||
cpSpaceSegmentQuery(m_handle, { from.x, from.y }, { to.x, to.y }, radius, filter, callback, hitInfos);
|
||||
|
||||
return hitInfos->size() != previousSize;
|
||||
}
|
||||
|
||||
bool ChipmunkPhysWorld2D::RaycastQueryFirst(const Vector2f& from, const Vector2f& to, float radius, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, RaycastHit* hitInfo)
|
||||
{
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
|
||||
if (hitInfo)
|
||||
{
|
||||
cpSegmentQueryInfo queryInfo;
|
||||
|
||||
if (cpSpaceSegmentQueryFirst(m_handle, { from.x, from.y }, { to.x, to.y }, radius, filter, &queryInfo))
|
||||
{
|
||||
hitInfo->fraction = float(queryInfo.alpha);
|
||||
hitInfo->hitNormal.Set(Vector2<cpFloat>(queryInfo.normal.x, queryInfo.normal.y));
|
||||
hitInfo->hitPos.Set(Vector2<cpFloat>(queryInfo.point.x, queryInfo.point.y));
|
||||
hitInfo->nearestBody = static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(queryInfo.shape));
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cpSpaceSegmentQueryFirst(m_handle, { from.x, from.y }, { to.x, to.y }, radius, filter, nullptr))
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RegionQuery(const Rectf& boundingBox, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, const FunctionRef<void(ChipmunkRigidBody2D*)>& callback)
|
||||
{
|
||||
using CallbackType = std::remove_reference_t<decltype(callback)>;
|
||||
|
||||
auto cpCallback = [](cpShape* shape, void* data)
|
||||
{
|
||||
CallbackType& callback = *static_cast<CallbackType*>(data);
|
||||
callback(static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape)));
|
||||
};
|
||||
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
cpSpaceBBQuery(m_handle, cpBBNew(boundingBox.x, boundingBox.y, boundingBox.x + boundingBox.width, boundingBox.y + boundingBox.height), filter, cpCallback, const_cast<void*>(static_cast<const void*>(&callback)));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RegionQuery(const Rectf& boundingBox, UInt32 collisionGroup, UInt32 categoryMask, UInt32 collisionMask, std::vector<ChipmunkRigidBody2D*>* bodies)
|
||||
{
|
||||
using ResultType = decltype(bodies);
|
||||
|
||||
auto callback = [] (cpShape* shape, void* data)
|
||||
{
|
||||
ResultType results = static_cast<ResultType>(data);
|
||||
results->push_back(static_cast<ChipmunkRigidBody2D*>(cpShapeGetUserData(shape)));
|
||||
};
|
||||
|
||||
cpShapeFilter filter = cpShapeFilterNew(collisionGroup, categoryMask, collisionMask);
|
||||
cpSpaceBBQuery(m_handle, cpBBNew(boundingBox.x, boundingBox.y, boundingBox.x + boundingBox.width, boundingBox.y + boundingBox.height), filter, callback, bodies);
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RegisterCallbacks(unsigned int collisionId, Callback callbacks)
|
||||
{
|
||||
InitCallbacks(cpSpaceAddWildcardHandler(m_handle, collisionId), std::move(callbacks));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RegisterCallbacks(unsigned int collisionIdA, unsigned int collisionIdB, Callback callbacks)
|
||||
{
|
||||
InitCallbacks(cpSpaceAddCollisionHandler(m_handle, collisionIdA, collisionIdB), std::move(callbacks));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetDamping(float dampingValue)
|
||||
{
|
||||
cpSpaceSetDamping(m_handle, dampingValue);
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetGravity(const Vector2f& gravity)
|
||||
{
|
||||
cpSpaceSetGravity(m_handle, cpv(gravity.x, gravity.y));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetIterationCount(std::size_t iterationCount)
|
||||
{
|
||||
cpSpaceSetIterations(m_handle, SafeCast<int>(iterationCount));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetMaxStepCount(std::size_t maxStepCount)
|
||||
{
|
||||
m_maxStepCount = maxStepCount;
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetSleepTime(Time sleepTime)
|
||||
{
|
||||
if (sleepTime > Time::Zero())
|
||||
cpSpaceSetSleepTimeThreshold(m_handle, sleepTime.AsSeconds<cpFloat>());
|
||||
else
|
||||
cpSpaceSetSleepTimeThreshold(m_handle, std::numeric_limits<cpFloat>::infinity());
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::SetStepSize(Time stepSize)
|
||||
{
|
||||
m_stepSize = stepSize;
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::Step(Time timestep)
|
||||
{
|
||||
m_timestepAccumulator += timestep;
|
||||
|
||||
std::size_t stepCount = std::min(static_cast<std::size_t>(static_cast<Int64>(m_timestepAccumulator / m_stepSize)), m_maxStepCount);
|
||||
float invStepCount = 1.f / stepCount;
|
||||
|
||||
cpFloat dt = m_stepSize.AsSeconds<float>(); //< FIXME: AsSeconds<cpFloat> is more precise but it fails unit tests on Linux
|
||||
for (std::size_t i = 0; i < stepCount; ++i)
|
||||
{
|
||||
OnPhysWorld2DPreStep(this, invStepCount);
|
||||
|
||||
cpSpaceStep(m_handle, dt);
|
||||
|
||||
OnPhysWorld2DPostStep(this, invStepCount);
|
||||
if (!m_rigidPostSteps.empty())
|
||||
{
|
||||
for (const auto& pair : m_rigidPostSteps)
|
||||
{
|
||||
for (const auto& step : pair.second.funcs)
|
||||
step(pair.first);
|
||||
}
|
||||
|
||||
m_rigidPostSteps.clear();
|
||||
}
|
||||
|
||||
m_timestepAccumulator -= m_stepSize;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::UseSpatialHash(float cellSize, std::size_t entityCount)
|
||||
{
|
||||
cpSpaceUseSpatialHash(m_handle, cpFloat(cellSize), int(entityCount));
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::InitCallbacks(cpCollisionHandler* handler, Callback callbacks)
|
||||
{
|
||||
auto it = m_callbacks.find(handler);
|
||||
if (it == m_callbacks.end())
|
||||
it = m_callbacks.emplace(handler, std::make_unique<Callback>(std::move(callbacks))).first;
|
||||
else
|
||||
it->second = std::make_unique<Callback>(std::move(callbacks));
|
||||
|
||||
Callback* callbackFunctions = it->second.get();
|
||||
handler->userData = callbackFunctions;
|
||||
|
||||
if (callbackFunctions->startCallback)
|
||||
{
|
||||
handler->beginFunc = [](cpArbiter* arb, cpSpace* space, void* data) -> cpBool
|
||||
{
|
||||
cpBody* firstBody;
|
||||
cpBody* secondBody;
|
||||
cpArbiterGetBodies(arb, &firstBody, &secondBody);
|
||||
|
||||
ChipmunkPhysWorld2D* world = static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(space));
|
||||
ChipmunkRigidBody2D* firstRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(firstBody));
|
||||
ChipmunkRigidBody2D* secondRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(secondBody));
|
||||
|
||||
ChipmunkArbiter2D arbiter(arb);
|
||||
|
||||
const Callback* customCallbacks = static_cast<const Callback*>(data);
|
||||
if (customCallbacks->startCallback(*world, arbiter, *firstRigidBody, *secondRigidBody, customCallbacks->userdata))
|
||||
return cpTrue;
|
||||
else
|
||||
return cpFalse;
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
handler->beginFunc = [](cpArbiter*, cpSpace*, void*) -> cpBool
|
||||
{
|
||||
return cpTrue;
|
||||
};
|
||||
}
|
||||
|
||||
if (callbackFunctions->endCallback)
|
||||
{
|
||||
handler->separateFunc = [](cpArbiter* arb, cpSpace* space, void* data)
|
||||
{
|
||||
cpBody* firstBody;
|
||||
cpBody* secondBody;
|
||||
cpArbiterGetBodies(arb, &firstBody, &secondBody);
|
||||
|
||||
ChipmunkPhysWorld2D* world = static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(space));
|
||||
ChipmunkRigidBody2D* firstRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(firstBody));
|
||||
ChipmunkRigidBody2D* secondRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(secondBody));
|
||||
|
||||
ChipmunkArbiter2D arbiter(arb);
|
||||
|
||||
const Callback* customCallbacks = static_cast<const Callback*>(data);
|
||||
customCallbacks->endCallback(*world, arbiter, *firstRigidBody, *secondRigidBody, customCallbacks->userdata);
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
handler->separateFunc = [](cpArbiter*, cpSpace*, void*)
|
||||
{
|
||||
};
|
||||
}
|
||||
|
||||
if (callbackFunctions->preSolveCallback)
|
||||
{
|
||||
handler->preSolveFunc = [](cpArbiter* arb, cpSpace* space, void* data) -> cpBool
|
||||
{
|
||||
cpBody* firstBody;
|
||||
cpBody* secondBody;
|
||||
cpArbiterGetBodies(arb, &firstBody, &secondBody);
|
||||
|
||||
ChipmunkPhysWorld2D* world = static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(space));
|
||||
ChipmunkRigidBody2D* firstRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(firstBody));
|
||||
ChipmunkRigidBody2D* secondRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(secondBody));
|
||||
|
||||
ChipmunkArbiter2D arbiter(arb);
|
||||
|
||||
const Callback* customCallbacks = static_cast<const Callback*>(data);
|
||||
if (customCallbacks->preSolveCallback(*world, arbiter, *firstRigidBody, *secondRigidBody, customCallbacks->userdata))
|
||||
return cpTrue;
|
||||
else
|
||||
return cpFalse;
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
handler->preSolveFunc = [](cpArbiter*, cpSpace*, void*) -> cpBool
|
||||
{
|
||||
return cpTrue;
|
||||
};
|
||||
}
|
||||
|
||||
if (callbackFunctions->postSolveCallback)
|
||||
{
|
||||
handler->postSolveFunc = [](cpArbiter* arb, cpSpace* space, void *data)
|
||||
{
|
||||
cpBody* firstBody;
|
||||
cpBody* secondBody;
|
||||
cpArbiterGetBodies(arb, &firstBody, &secondBody);
|
||||
|
||||
ChipmunkPhysWorld2D* world = static_cast<ChipmunkPhysWorld2D*>(cpSpaceGetUserData(space));
|
||||
ChipmunkRigidBody2D* firstRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(firstBody));
|
||||
ChipmunkRigidBody2D* secondRigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(secondBody));
|
||||
|
||||
ChipmunkArbiter2D arbiter(arb);
|
||||
|
||||
const Callback* customCallbacks = static_cast<const Callback*>(data);
|
||||
customCallbacks->postSolveCallback(*world, arbiter, *firstRigidBody, *secondRigidBody, customCallbacks->userdata);
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
handler->postSolveFunc = [](cpArbiter*, cpSpace*, void*)
|
||||
{
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::OnRigidBodyMoved(ChipmunkRigidBody2D* oldPointer, ChipmunkRigidBody2D* newPointer)
|
||||
{
|
||||
auto it = m_rigidPostSteps.find(oldPointer);
|
||||
if (it == m_rigidPostSteps.end())
|
||||
return; //< Shouldn't happen
|
||||
|
||||
m_rigidPostSteps.emplace(std::make_pair(newPointer, std::move(it->second)));
|
||||
m_rigidPostSteps.erase(oldPointer);
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::OnRigidBodyRelease(ChipmunkRigidBody2D* rigidBody)
|
||||
{
|
||||
m_rigidPostSteps.erase(rigidBody);
|
||||
}
|
||||
|
||||
void ChipmunkPhysWorld2D::RegisterPostStep(ChipmunkRigidBody2D* rigidBody, PostStep&& func)
|
||||
{
|
||||
// If space isn't locked, no need to wait
|
||||
if (!cpSpaceIsLocked(m_handle))
|
||||
{
|
||||
func(rigidBody);
|
||||
return;
|
||||
}
|
||||
|
||||
auto it = m_rigidPostSteps.find(rigidBody);
|
||||
if (it == m_rigidPostSteps.end())
|
||||
{
|
||||
PostStepContainer postStep;
|
||||
postStep.onMovedSlot.Connect(rigidBody->OnRigidBody2DMove, this, &ChipmunkPhysWorld2D::OnRigidBodyMoved);
|
||||
postStep.onReleaseSlot.Connect(rigidBody->OnRigidBody2DRelease, this, &ChipmunkPhysWorld2D::OnRigidBodyRelease);
|
||||
|
||||
it = m_rigidPostSteps.insert(std::make_pair(rigidBody, std::move(postStep))).first;
|
||||
}
|
||||
|
||||
it->second.funcs.emplace_back(std::move(func));
|
||||
}
|
||||
}
|
||||
16
src/Nazara/ChipmunkPhysics2D/ChipmunkPhysics2D.cpp
Normal file
16
src/Nazara/ChipmunkPhysics2D/ChipmunkPhysics2D.cpp
Normal file
@@ -0,0 +1,16 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkPhysics2D.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
ChipmunkPhysics2D::ChipmunkPhysics2D(Config /*config*/) :
|
||||
ModuleBase("ChipmunkPhysics2D", this)
|
||||
{
|
||||
}
|
||||
|
||||
ChipmunkPhysics2D* ChipmunkPhysics2D::s_instance = nullptr;
|
||||
}
|
||||
726
src/Nazara/ChipmunkPhysics2D/ChipmunkRigidBody2D.cpp
Normal file
726
src/Nazara/ChipmunkPhysics2D/ChipmunkRigidBody2D.cpp
Normal file
@@ -0,0 +1,726 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkRigidBody2D.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkArbiter2D.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/ChipmunkPhysWorld2D.hpp>
|
||||
#include <chipmunk/chipmunk.h>
|
||||
#include <chipmunk/chipmunk_private.h>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
ChipmunkRigidBody2D::ChipmunkRigidBody2D(ChipmunkPhysWorld2D* world, float mass) :
|
||||
ChipmunkRigidBody2D(world, mass, nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D::ChipmunkRigidBody2D(ChipmunkPhysWorld2D* world, float mass, std::shared_ptr<ChipmunkCollider2D> geom) :
|
||||
m_positionOffset(Vector2f::Zero()),
|
||||
m_geom(),
|
||||
m_userData(nullptr),
|
||||
m_world(world),
|
||||
m_isRegistered(false),
|
||||
m_isSimulationEnabled(true),
|
||||
m_isStatic(false),
|
||||
m_gravityFactor(1.f),
|
||||
m_mass(mass)
|
||||
{
|
||||
NazaraAssert(m_world, "Invalid world");
|
||||
|
||||
m_handle = Create(mass);
|
||||
SetGeom(std::move(geom));
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D::ChipmunkRigidBody2D(const ChipmunkRigidBody2D& object) :
|
||||
m_positionOffset(object.m_positionOffset),
|
||||
m_geom(object.m_geom),
|
||||
m_userData(object.m_userData),
|
||||
m_world(object.m_world),
|
||||
m_isRegistered(false),
|
||||
m_isSimulationEnabled(true),
|
||||
m_isStatic(object.m_isStatic),
|
||||
m_gravityFactor(object.m_gravityFactor),
|
||||
m_mass(object.GetMass())
|
||||
{
|
||||
NazaraAssert(m_world, "Invalid world");
|
||||
NazaraAssert(m_geom, "Invalid geometry");
|
||||
|
||||
m_handle = Create(m_mass, object.GetMomentOfInertia());
|
||||
SetGeom(object.GetGeom(), false, false);
|
||||
|
||||
CopyBodyData(object.GetHandle(), m_handle);
|
||||
|
||||
for (std::size_t i = 0; i < m_shapes.size(); ++i)
|
||||
{
|
||||
CopyShapeData(object.m_shapes[i], m_shapes[i]);
|
||||
m_shapes[i]->bb = cpShapeCacheBB(object.m_shapes[i]);
|
||||
}
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D::ChipmunkRigidBody2D(ChipmunkRigidBody2D&& object) noexcept :
|
||||
OnRigidBody2DMove(std::move(object.OnRigidBody2DMove)),
|
||||
OnRigidBody2DRelease(std::move(object.OnRigidBody2DRelease)),
|
||||
m_positionOffset(std::move(object.m_positionOffset)),
|
||||
m_shapes(std::move(object.m_shapes)),
|
||||
m_geom(std::move(object.m_geom)),
|
||||
m_handle(object.m_handle),
|
||||
m_userData(object.m_userData),
|
||||
m_world(object.m_world),
|
||||
m_isRegistered(object.m_isRegistered),
|
||||
m_isSimulationEnabled(object.m_isSimulationEnabled),
|
||||
m_isStatic(object.m_isStatic),
|
||||
m_gravityFactor(object.m_gravityFactor),
|
||||
m_mass(object.m_mass)
|
||||
{
|
||||
cpBodySetUserData(m_handle, this);
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetUserData(shape, this);
|
||||
|
||||
object.m_handle = nullptr;
|
||||
|
||||
OnRigidBody2DMove(&object, this);
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D::~ChipmunkRigidBody2D()
|
||||
{
|
||||
OnRigidBody2DRelease(this);
|
||||
|
||||
Destroy();
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::AddForce(const Vector2f& force, CoordSys coordSys)
|
||||
{
|
||||
return AddForce(force, GetMassCenter(coordSys), coordSys);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::AddForce(const Vector2f& force, const Vector2f& point, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
cpBodyApplyForceAtWorldPoint(m_handle, cpv(force.x, force.y), cpv(point.x, point.y));
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
cpBodyApplyForceAtLocalPoint(m_handle, cpv(force.x, force.y), cpv(point.x, point.y));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::AddImpulse(const Vector2f& impulse, CoordSys coordSys)
|
||||
{
|
||||
return AddImpulse(impulse, GetMassCenter(coordSys), coordSys);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::AddImpulse(const Vector2f& impulse, const Vector2f& point, CoordSys coordSys)
|
||||
{
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
cpBodyApplyImpulseAtWorldPoint(m_handle, cpv(impulse.x, impulse.y), cpv(point.x, point.y));
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
cpBodyApplyImpulseAtLocalPoint(m_handle, cpv(impulse.x, impulse.y), cpv(point.x, point.y));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::AddTorque(const RadianAnglef& torque)
|
||||
{
|
||||
cpBodySetTorque(m_handle, cpBodyGetTorque(m_handle) + torque.value);
|
||||
}
|
||||
|
||||
bool ChipmunkRigidBody2D::ClosestPointQuery(const Vector2f& position, Vector2f* closestPoint, float* closestDistance) const
|
||||
{
|
||||
cpVect pos = cpv(cpFloat(position.x), cpFloat(position.y));
|
||||
|
||||
float minDistance = std::numeric_limits<float>::infinity();
|
||||
Vector2f closest;
|
||||
for (cpShape* shape : m_shapes)
|
||||
{
|
||||
cpPointQueryInfo result;
|
||||
cpShapePointQuery(shape, pos, &result);
|
||||
|
||||
float resultDistance = float(result.distance);
|
||||
if (resultDistance < minDistance)
|
||||
{
|
||||
closest.Set(float(result.point.x), float(result.point.y));
|
||||
minDistance = resultDistance;
|
||||
}
|
||||
}
|
||||
|
||||
if (std::isinf(minDistance))
|
||||
return false;
|
||||
|
||||
if (closestPoint)
|
||||
*closestPoint = closest;
|
||||
|
||||
if (closestDistance)
|
||||
*closestDistance = minDistance;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::EnableSimulation(bool simulation)
|
||||
{
|
||||
if (m_isSimulationEnabled != simulation)
|
||||
{
|
||||
m_isSimulationEnabled = simulation;
|
||||
|
||||
if (simulation)
|
||||
RegisterToSpace();
|
||||
else
|
||||
UnregisterFromSpace();
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::ForEachArbiter(std::function<void(ChipmunkArbiter2D&)> callback)
|
||||
{
|
||||
using CallbackType = decltype(callback);
|
||||
|
||||
auto RealCallback = [](cpBody* /*body*/, cpArbiter* arbiter, void* data)
|
||||
{
|
||||
CallbackType& cb = *static_cast<CallbackType*>(data);
|
||||
|
||||
ChipmunkArbiter2D nzArbiter(arbiter);
|
||||
cb(nzArbiter);
|
||||
};
|
||||
|
||||
cpBodyEachArbiter(m_handle, RealCallback, &callback);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::ForceSleep()
|
||||
{
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
if (cpBodyGetType(body->GetHandle()) == CP_BODY_TYPE_DYNAMIC)
|
||||
cpBodySleep(body->GetHandle());
|
||||
});
|
||||
}
|
||||
|
||||
Rectf ChipmunkRigidBody2D::GetAABB() const
|
||||
{
|
||||
if (m_shapes.empty())
|
||||
return Rectf::Zero();
|
||||
|
||||
auto it = m_shapes.begin();
|
||||
cpBB bb = cpShapeGetBB(*it++);
|
||||
for (; it != m_shapes.end(); ++it)
|
||||
bb = cpBBMerge(bb, cpShapeGetBB(*it));
|
||||
|
||||
return Rectf(Rect<cpFloat>(bb.l, bb.b, bb.r - bb.l, bb.t - bb.b));
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkRigidBody2D::GetAngularVelocity() const
|
||||
{
|
||||
return float(cpBodyGetAngularVelocity(m_handle));
|
||||
}
|
||||
|
||||
float ChipmunkRigidBody2D::GetElasticity(std::size_t shapeIndex) const
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
return float(cpShapeGetElasticity(m_shapes[shapeIndex]));
|
||||
}
|
||||
|
||||
float ChipmunkRigidBody2D::GetFriction(std::size_t shapeIndex) const
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
return float(cpShapeGetFriction(m_shapes[shapeIndex]));
|
||||
}
|
||||
|
||||
const std::shared_ptr<ChipmunkCollider2D>& ChipmunkRigidBody2D::GetGeom() const
|
||||
{
|
||||
return m_geom;
|
||||
}
|
||||
|
||||
cpBody* ChipmunkRigidBody2D::GetHandle() const
|
||||
{
|
||||
return m_handle;
|
||||
}
|
||||
|
||||
float ChipmunkRigidBody2D::GetMass() const
|
||||
{
|
||||
return m_mass;
|
||||
}
|
||||
|
||||
Vector2f ChipmunkRigidBody2D::GetMassCenter(CoordSys coordSys) const
|
||||
{
|
||||
cpVect massCenter = cpBodyGetCenterOfGravity(m_handle);
|
||||
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
massCenter = cpBodyLocalToWorld(m_handle, massCenter);
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
break; // Nothing to do
|
||||
}
|
||||
|
||||
return Vector2f(static_cast<float>(massCenter.x), static_cast<float>(massCenter.y));
|
||||
}
|
||||
|
||||
float ChipmunkRigidBody2D::GetMomentOfInertia() const
|
||||
{
|
||||
return float(cpBodyGetMoment(m_handle));
|
||||
}
|
||||
|
||||
Vector2f ChipmunkRigidBody2D::GetPosition() const
|
||||
{
|
||||
cpVect pos = cpBodyLocalToWorld(m_handle, cpv(-m_positionOffset.x, -m_positionOffset.y));
|
||||
return Vector2f(static_cast<float>(pos.x), static_cast<float>(pos.y));
|
||||
}
|
||||
|
||||
RadianAnglef ChipmunkRigidBody2D::GetRotation() const
|
||||
{
|
||||
return float(cpBodyGetAngle(m_handle));
|
||||
}
|
||||
|
||||
std::size_t ChipmunkRigidBody2D::GetShapeIndex(cpShape* shape) const
|
||||
{
|
||||
auto it = std::find(m_shapes.begin(), m_shapes.end(), shape);
|
||||
if (it == m_shapes.end())
|
||||
return InvalidShapeIndex;
|
||||
|
||||
return std::distance(m_shapes.begin(), it);
|
||||
}
|
||||
|
||||
Vector2f ChipmunkRigidBody2D::GetSurfaceVelocity(std::size_t shapeIndex) const
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
cpVect vel = cpShapeGetSurfaceVelocity(m_shapes[shapeIndex]);
|
||||
return Vector2f(static_cast<float>(vel.x), static_cast<float>(vel.y));
|
||||
}
|
||||
|
||||
void* ChipmunkRigidBody2D::GetUserdata() const
|
||||
{
|
||||
return m_userData;
|
||||
}
|
||||
|
||||
Vector2f ChipmunkRigidBody2D::GetVelocity() const
|
||||
{
|
||||
cpVect vel = cpBodyGetVelocity(m_handle);
|
||||
return Vector2f(static_cast<float>(vel.x), static_cast<float>(vel.y));
|
||||
}
|
||||
|
||||
const ChipmunkRigidBody2D::VelocityFunc& ChipmunkRigidBody2D::GetVelocityFunction() const
|
||||
{
|
||||
return m_velocityFunc;
|
||||
}
|
||||
|
||||
ChipmunkPhysWorld2D* ChipmunkRigidBody2D::GetWorld() const
|
||||
{
|
||||
return m_world;
|
||||
}
|
||||
|
||||
bool ChipmunkRigidBody2D::IsKinematic() const
|
||||
{
|
||||
return m_mass <= 0.f;
|
||||
}
|
||||
|
||||
bool ChipmunkRigidBody2D::IsSimulationEnabled() const
|
||||
{
|
||||
return m_isSimulationEnabled;
|
||||
}
|
||||
|
||||
bool ChipmunkRigidBody2D::IsSleeping() const
|
||||
{
|
||||
return cpBodyIsSleeping(m_handle) != 0;
|
||||
}
|
||||
|
||||
bool ChipmunkRigidBody2D::IsStatic() const
|
||||
{
|
||||
return m_isStatic;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::ResetVelocityFunction()
|
||||
{
|
||||
m_handle->velocity_func = cpBodyUpdateVelocity;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetAngularVelocity(const RadianAnglef& angularVelocity)
|
||||
{
|
||||
cpBodySetAngularVelocity(m_handle, angularVelocity.value);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetElasticity(float friction)
|
||||
{
|
||||
cpFloat frict(friction);
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetElasticity(shape, frict);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetElasticity(std::size_t shapeIndex, float friction)
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
cpShapeSetElasticity(m_shapes[shapeIndex], cpFloat(friction));
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetFriction(float friction)
|
||||
{
|
||||
cpFloat frict(friction);
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetFriction(shape, frict);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetFriction(std::size_t shapeIndex, float friction)
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
cpShapeSetFriction(m_shapes[shapeIndex], cpFloat(friction));
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetGeom(std::shared_ptr<ChipmunkCollider2D> geom, bool recomputeMoment, bool recomputeMassCenter)
|
||||
{
|
||||
// We have no public way of getting rid of an existing geom without removing the whole body
|
||||
// So let's save some attributes of the body, destroy it and rebuild it
|
||||
if (m_geom)
|
||||
{
|
||||
cpFloat mass = cpBodyGetMass(m_handle);
|
||||
cpFloat moment = cpBodyGetMoment(m_handle);
|
||||
|
||||
cpBody* newHandle = Create(static_cast<float>(mass), static_cast<float>(moment));
|
||||
|
||||
CopyBodyData(m_handle, newHandle);
|
||||
|
||||
Destroy();
|
||||
|
||||
m_handle = newHandle;
|
||||
}
|
||||
|
||||
if (geom)
|
||||
m_geom = std::move(geom);
|
||||
else
|
||||
m_geom = std::make_shared<ChipmunkNullCollider2D>();
|
||||
|
||||
m_geom->GenerateShapes(m_handle, &m_shapes);
|
||||
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetUserData(shape, this);
|
||||
|
||||
if (m_isSimulationEnabled)
|
||||
RegisterToSpace();
|
||||
|
||||
if (recomputeMoment)
|
||||
{
|
||||
if (!IsStatic() && !IsKinematic())
|
||||
cpBodySetMoment(m_handle, m_geom->ComputeMomentOfInertia(m_mass));
|
||||
}
|
||||
|
||||
if (recomputeMassCenter)
|
||||
SetMassCenter(m_geom->ComputeCenterOfMass());
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetMass(float mass, bool recomputeMoment)
|
||||
{
|
||||
if (m_mass > 0.f)
|
||||
{
|
||||
if (mass > 0.f)
|
||||
{
|
||||
m_world->RegisterPostStep(this, [mass, recomputeMoment](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
cpBodySetMass(body->GetHandle(), mass);
|
||||
|
||||
if (recomputeMoment)
|
||||
cpBodySetMoment(body->GetHandle(), body->GetGeom()->ComputeMomentOfInertia(mass));
|
||||
});
|
||||
}
|
||||
else
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body) { cpBodySetType(body->GetHandle(), (body->IsStatic()) ? CP_BODY_TYPE_STATIC : CP_BODY_TYPE_KINEMATIC); } );
|
||||
}
|
||||
else if (mass > 0.f)
|
||||
{
|
||||
m_world->RegisterPostStep(this, [mass, recomputeMoment](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
if (cpBodyGetType(body->GetHandle()) != CP_BODY_TYPE_DYNAMIC)
|
||||
{
|
||||
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_DYNAMIC);
|
||||
cpBodySetMass(body->GetHandle(), mass);
|
||||
|
||||
if (recomputeMoment)
|
||||
cpBodySetMoment(body->GetHandle(), body->GetGeom()->ComputeMomentOfInertia(mass));
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
m_mass = mass;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetMassCenter(const Vector2f& center, CoordSys coordSys)
|
||||
{
|
||||
cpVect massCenter = cpv(center.x, center.y);
|
||||
|
||||
switch (coordSys)
|
||||
{
|
||||
case CoordSys::Global:
|
||||
massCenter = cpBodyWorldToLocal(m_handle, massCenter);
|
||||
break;
|
||||
|
||||
case CoordSys::Local:
|
||||
break; // Nothing to do
|
||||
}
|
||||
|
||||
cpBodySetCenterOfGravity(m_handle, massCenter);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetMomentOfInertia(float moment)
|
||||
{
|
||||
// Even though Chipmunk allows us to change this anytime, we need to do it in a post-step to prevent other post-steps to override this
|
||||
m_world->RegisterPostStep(this, [moment] (ChipmunkRigidBody2D* body)
|
||||
{
|
||||
cpBodySetMoment(body->GetHandle(), moment);
|
||||
});
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetPosition(const Vector2f& position)
|
||||
{
|
||||
// Use cpTransformVect to rotate/scale the position offset
|
||||
cpBodySetPosition(m_handle, cpvadd(cpv(position.x, position.y), cpTransformVect(m_handle->transform, cpv(m_positionOffset.x, m_positionOffset.y))));
|
||||
if (m_isStatic)
|
||||
{
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetPositionOffset(const Vector2f& offset)
|
||||
{
|
||||
Vector2f position = GetPosition();
|
||||
m_positionOffset = offset;
|
||||
SetPosition(position);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetRotation(const RadianAnglef& rotation)
|
||||
{
|
||||
cpBodySetAngle(m_handle, rotation.value);
|
||||
if (m_isStatic)
|
||||
{
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetSurfaceVelocity(const Vector2f& surfaceVelocity)
|
||||
{
|
||||
Vector2<cpFloat> velocity(surfaceVelocity.x, surfaceVelocity.y);
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetSurfaceVelocity(shape, cpv(velocity.x, velocity.y));
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetSurfaceVelocity(std::size_t shapeIndex, const Vector2f& surfaceVelocity)
|
||||
{
|
||||
assert(shapeIndex < m_shapes.size());
|
||||
cpShapeSetSurfaceVelocity(m_shapes[shapeIndex], cpv(cpFloat(surfaceVelocity.x), cpFloat(surfaceVelocity.y)));
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetStatic(bool setStaticBody)
|
||||
{
|
||||
m_isStatic = setStaticBody;
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
if (body->IsStatic())
|
||||
{
|
||||
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_STATIC);
|
||||
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
|
||||
}
|
||||
else if (cpBodyGetMass(body->GetHandle()) > 0.f)
|
||||
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_KINEMATIC);
|
||||
else
|
||||
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_DYNAMIC);
|
||||
});
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetUserdata(void* ud)
|
||||
{
|
||||
m_userData = ud;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetVelocity(const Vector2f& velocity)
|
||||
{
|
||||
cpBodySetVelocity(m_handle, cpv(velocity.x, velocity.y));
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::SetVelocityFunction(VelocityFunc velocityFunc)
|
||||
{
|
||||
m_velocityFunc = std::move(velocityFunc);
|
||||
|
||||
if (m_velocityFunc)
|
||||
{
|
||||
m_handle->velocity_func = [](cpBody* body, cpVect gravity, cpFloat damping, cpFloat dt)
|
||||
{
|
||||
ChipmunkRigidBody2D* rigidBody = static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(body));
|
||||
const auto& callback = rigidBody->GetVelocityFunction();
|
||||
assert(callback);
|
||||
|
||||
callback(*rigidBody, Vector2f(float(gravity.x), float(gravity.y)), float(damping), float(dt));
|
||||
};
|
||||
}
|
||||
else
|
||||
m_handle->velocity_func = cpBodyUpdateVelocity;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::TeleportTo(const Vector2f& position, const RadianAnglef& rotation)
|
||||
{
|
||||
// Use cpTransformVect to rotate/scale the position offset
|
||||
cpBodySetPosition(m_handle, cpvadd(cpv(position.x, position.y), cpTransformVect(m_handle->transform, cpv(m_positionOffset.x, m_positionOffset.y))));
|
||||
cpBodySetAngle(m_handle, rotation.value);
|
||||
if (m_isStatic)
|
||||
{
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::UpdateVelocity(const Vector2f & gravity, float damping, float deltaTime)
|
||||
{
|
||||
cpBodyUpdateVelocity(m_handle, cpv(gravity.x, gravity.y), damping, deltaTime);
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::Wakeup()
|
||||
{
|
||||
m_world->RegisterPostStep(this, [](ChipmunkRigidBody2D* body)
|
||||
{
|
||||
if (cpBodyGetType(body->GetHandle()) != CP_BODY_TYPE_STATIC)
|
||||
cpBodyActivate(body->GetHandle());
|
||||
else
|
||||
cpBodyActivateStatic(body->GetHandle(), nullptr);
|
||||
});
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D& ChipmunkRigidBody2D::operator=(const ChipmunkRigidBody2D& object)
|
||||
{
|
||||
ChipmunkRigidBody2D physObj(object);
|
||||
return operator=(std::move(physObj));
|
||||
}
|
||||
|
||||
ChipmunkRigidBody2D& ChipmunkRigidBody2D::operator=(ChipmunkRigidBody2D&& object)
|
||||
{
|
||||
Destroy();
|
||||
|
||||
OnRigidBody2DMove = std::move(object.OnRigidBody2DMove);
|
||||
OnRigidBody2DRelease = std::move(object.OnRigidBody2DRelease);
|
||||
|
||||
m_handle = object.m_handle;
|
||||
m_isRegistered = object.m_isRegistered;
|
||||
m_isSimulationEnabled = object.m_isSimulationEnabled;
|
||||
m_isStatic = object.m_isStatic;
|
||||
m_geom = std::move(object.m_geom);
|
||||
m_gravityFactor = object.m_gravityFactor;
|
||||
m_mass = object.m_mass;
|
||||
m_positionOffset = object.m_positionOffset;
|
||||
m_shapes = std::move(object.m_shapes);
|
||||
m_userData = object.m_userData;
|
||||
m_velocityFunc = std::move(object.m_velocityFunc);
|
||||
m_world = object.m_world;
|
||||
|
||||
if (m_handle)
|
||||
{
|
||||
cpBodySetUserData(m_handle, this);
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeSetUserData(shape, this);
|
||||
}
|
||||
|
||||
object.m_handle = nullptr;
|
||||
|
||||
OnRigidBody2DMove(&object, this);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::Destroy()
|
||||
{
|
||||
UnregisterFromSpace();
|
||||
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpShapeFree(shape);
|
||||
|
||||
if (m_handle)
|
||||
{
|
||||
cpBodyFree(m_handle);
|
||||
m_handle = nullptr;
|
||||
}
|
||||
|
||||
m_shapes.clear();
|
||||
}
|
||||
|
||||
cpBody* ChipmunkRigidBody2D::Create(float mass, float moment)
|
||||
{
|
||||
cpBody* handle;
|
||||
if (IsKinematic())
|
||||
{
|
||||
if (IsStatic())
|
||||
handle = cpBodyNewStatic();
|
||||
else
|
||||
handle = cpBodyNewKinematic();
|
||||
}
|
||||
else
|
||||
handle = cpBodyNew(mass, moment);
|
||||
|
||||
cpBodySetUserData(handle, this);
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::RegisterToSpace()
|
||||
{
|
||||
if (!m_isRegistered)
|
||||
{
|
||||
cpSpace* space = m_world->GetHandle();
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpSpaceAddShape(space, shape);
|
||||
|
||||
if (m_handle)
|
||||
cpSpaceAddBody(space, m_handle);
|
||||
|
||||
m_isRegistered = true;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::UnregisterFromSpace()
|
||||
{
|
||||
if (m_isRegistered)
|
||||
{
|
||||
cpSpace* space = m_world->GetHandle();
|
||||
for (cpShape* shape : m_shapes)
|
||||
cpSpaceRemoveShape(space, shape);
|
||||
|
||||
if (m_handle)
|
||||
cpSpaceRemoveBody(space, m_handle);
|
||||
|
||||
m_isRegistered = false;
|
||||
}
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::CopyBodyData(cpBody* from, cpBody* to)
|
||||
{
|
||||
cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
|
||||
|
||||
cpBodySetAngle(to, cpBodyGetAngle(from));
|
||||
cpBodySetAngularVelocity(to, cpBodyGetAngularVelocity(from));
|
||||
cpBodySetForce(to, cpBodyGetForce(from));
|
||||
cpBodySetPosition(to, cpBodyGetPosition(from));
|
||||
cpBodySetTorque(to, cpBodyGetTorque(from));
|
||||
cpBodySetVelocity(to, cpBodyGetVelocity(from));
|
||||
|
||||
cpBodySetType(to, cpBodyGetType(from));
|
||||
|
||||
to->velocity_func = from->velocity_func;
|
||||
}
|
||||
|
||||
void ChipmunkRigidBody2D::CopyShapeData(cpShape* from, cpShape* to)
|
||||
{
|
||||
cpShapeSetElasticity(to, cpShapeGetElasticity(from));
|
||||
cpShapeSetFriction(to, cpShapeGetFriction(from));
|
||||
cpShapeSetSurfaceVelocity(to, cpShapeGetSurfaceVelocity(from));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/Components/ChipmunkRigidBody2DComponent.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
|
||||
// This file is part of the "Nazara Engine - Physics2D module"
|
||||
// For conditions of distribution and use, see copyright notice in Config.hpp
|
||||
|
||||
#include <Nazara/ChipmunkPhysics2D/Systems/ChipmunkPhysics2DSystem.hpp>
|
||||
#include <Nazara/Utility/Components/NodeComponent.hpp>
|
||||
#include <Nazara/ChipmunkPhysics2D/Debug.hpp>
|
||||
|
||||
namespace Nz
|
||||
{
|
||||
namespace
|
||||
{
|
||||
inline RadianAnglef AngleFromQuaternion(const Quaternionf& quat)
|
||||
{
|
||||
float siny_cosp = 2.f * (quat.w * quat.z + quat.x * quat.y);
|
||||
float cosy_cosp = 1.f - 2.f * (quat.y * quat.y + quat.z * quat.z);
|
||||
|
||||
return std::atan2(siny_cosp, cosy_cosp); //<FIXME: not very efficient
|
||||
}
|
||||
}
|
||||
|
||||
ChipmunkPhysics2DSystem::ChipmunkPhysics2DSystem(entt::registry& registry) :
|
||||
m_registry(registry),
|
||||
m_physicsConstructObserver(m_registry, entt::collector.group<ChipmunkRigidBody2DComponent, NodeComponent>())
|
||||
{
|
||||
}
|
||||
|
||||
ChipmunkPhysics2DSystem::~ChipmunkPhysics2DSystem()
|
||||
{
|
||||
m_physicsConstructObserver.disconnect();
|
||||
|
||||
// Ensure every body is destroyed before world is
|
||||
auto rigidBodyView = m_registry.view<ChipmunkRigidBody2DComponent>();
|
||||
for (auto [entity, rigidBodyComponent] : rigidBodyView.each())
|
||||
rigidBodyComponent.Destroy();
|
||||
}
|
||||
|
||||
void ChipmunkPhysics2DSystem::Update(Time elapsedTime)
|
||||
{
|
||||
// Move newly-created physics entities to their node position/rotation
|
||||
m_physicsConstructObserver.each([&](entt::entity entity)
|
||||
{
|
||||
ChipmunkRigidBody2DComponent& entityPhysics = m_registry.get<ChipmunkRigidBody2DComponent>(entity);
|
||||
NodeComponent& entityNode = m_registry.get<NodeComponent>(entity);
|
||||
|
||||
entityPhysics.TeleportTo(Vector2f(entityNode.GetPosition()), AngleFromQuaternion(entityNode.GetRotation()));
|
||||
});
|
||||
|
||||
m_physWorld.Step(elapsedTime);
|
||||
|
||||
// Replicate rigid body position to their node components
|
||||
auto view = m_registry.view<NodeComponent, const ChipmunkRigidBody2DComponent>();
|
||||
for (auto [entity, nodeComponent, rigidBodyComponent] : view.each())
|
||||
{
|
||||
if (rigidBodyComponent.IsSleeping())
|
||||
continue;
|
||||
|
||||
nodeComponent.SetTransform(rigidBodyComponent.GetPosition(), rigidBodyComponent.GetRotation());
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user