From 13eab5d00dc82ae2c7c1ef32de79cd3b2d875241 Mon Sep 17 00:00:00 2001 From: Irlan Date: Sun, 23 Jun 2019 10:52:41 -0300 Subject: [PATCH] Small refactor. Allow cloth particle to collide with multiple shapes. This is a more general solution. --- include/bounce/cloth/cloth.h | 17 +- include/bounce/cloth/cloth_collision.h | 38 +++++ include/bounce/cloth/cloth_contact_manager.h | 21 ++- include/bounce/cloth/cloth_contact_solver.h | 2 +- .../cloth/cloth_particle_body_contact.h | 76 +++++++++ ...ct.h => cloth_particle_triangle_contact.h} | 6 +- include/bounce/cloth/cloth_solver.h | 2 +- include/bounce/cloth/cloth_triangle.h | 2 +- include/bounce/cloth/particle.h | 54 +----- include/bounce/dynamics/shapes/shape.h | 3 + src/bounce/cloth/cloth.cpp | 130 ++------------- src/bounce/cloth/cloth_contact_manager.cpp | 154 +++++++++++++++++- src/bounce/cloth/cloth_contact_solver.cpp | 34 ++-- .../cloth/cloth_particle_body_contact.cpp | 62 +++++++ ...pp => cloth_particle_triangle_contact.cpp} | 2 +- src/bounce/cloth/cloth_solver.cpp | 2 +- src/bounce/cloth/cloth_triangle.cpp | 1 + src/bounce/cloth/particle.cpp | 62 +++---- src/bounce/dynamics/shapes/shape.cpp | 5 + 19 files changed, 426 insertions(+), 247 deletions(-) create mode 100644 include/bounce/cloth/cloth_collision.h create mode 100644 include/bounce/cloth/cloth_particle_body_contact.h rename include/bounce/cloth/{cloth_contact.h => cloth_particle_triangle_contact.h} (94%) create mode 100644 src/bounce/cloth/cloth_particle_body_contact.cpp rename src/bounce/cloth/{cloth_contact.cpp => cloth_particle_triangle_contact.cpp} (98%) diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index 1f8e365..1b49752 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -26,16 +26,14 @@ #include class b3World; -class b3Shape; - -class b3Particle; -class b3ClothTriangle; - -class b3Force; -struct b3ParticleBodyContact; struct b3ParticleDef; +class b3Particle; + struct b3ForceDef; +class b3Force; + +class b3ClothTriangle; struct b3ClothMesh; @@ -44,8 +42,6 @@ class b3RayCastListener; struct b3RayCastInput; struct b3RayCastOutput; -struct b3ClothAABBProxy; - struct b3ClothRayCastSingleOutput { u32 triangle; @@ -161,9 +157,6 @@ private: // Compute mass of each particle. void ComputeMass(); - // Update particle-body contacts - void UpdateParticleBodyContacts(); - // Solve void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); diff --git a/include/bounce/cloth/cloth_collision.h b/include/bounce/cloth/cloth_collision.h new file mode 100644 index 0000000..58335f5 --- /dev/null +++ b/include/bounce/cloth/cloth_collision.h @@ -0,0 +1,38 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_COLLISION_H +#define B3_CLOTH_COLLISION_H + +#include + +// Cloth primitive type +enum b3ClothAABBProxyType +{ + e_particleProxy, + e_triangleProxy +}; + +// Cloth primitive broadphase proxy +struct b3ClothAABBProxy +{ + b3ClothAABBProxyType type; + void* owner; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_contact_manager.h b/include/bounce/cloth/cloth_contact_manager.h index 8f4517e..eccf419 100644 --- a/include/bounce/cloth/cloth_contact_manager.h +++ b/include/bounce/cloth/cloth_contact_manager.h @@ -19,7 +19,8 @@ #ifndef B3_CLOTH_CONTACT_MANAGER_H #define B3_CLOTH_CONTACT_MANAGER_H -#include +#include +#include #include #include #include @@ -32,21 +33,31 @@ class b3ClothContactManager public: b3ClothContactManager(); - // The broad-phase callback. - void AddPair(void* data1, void* data2); - void FindNewContacts(); + + void AddPair(void* data1, void* data2); + void FindNewClothContacts(); + + void AddPSPair(b3Particle* p1, b3Shape* s2); + void FindNewBodyContacts(); void UpdateContacts(); + void UpdateClothContacts(); + void UpdateBodyContacts(); b3ParticleTriangleContact* CreateParticleTriangleContact(); void Destroy(b3ParticleTriangleContact* c); + b3ParticleBodyContact* CreateParticleBodyContact(); + void Destroy(b3ParticleBodyContact* c); + b3BlockPool m_particleTriangleContactBlocks; - + b3BlockPool m_particleBodyContactBlocks; + b3Cloth* m_cloth; b3BroadPhase m_broadPhase; b3List2 m_particleTriangleContactList; + b3List2 m_particleBodyContactList; }; #endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h index 76f13bc..2b33265 100644 --- a/include/bounce/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/cloth_contact_solver.h @@ -27,7 +27,7 @@ class b3StackAllocator; class b3Particle; class b3Body; -struct b3ParticleBodyContact; +class b3ParticleBodyContact; class b3ParticleTriangleContact; struct b3ClothSolverBodyContactVelocityConstraint diff --git a/include/bounce/cloth/cloth_particle_body_contact.h b/include/bounce/cloth/cloth_particle_body_contact.h new file mode 100644 index 0000000..c774329 --- /dev/null +++ b/include/bounce/cloth/cloth_particle_body_contact.h @@ -0,0 +1,76 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_PARTICLE_BODY_CONTACT_H +#define B3_CLOTH_PARTICLE_BODY_CONTACT_H + +#include +#include +#include +#include + +class b3Particle; +class b3Shape; + +// A contact between a particle and a body +class b3ParticleBodyContact +{ +public: +private: + friend class b3List2; + friend class b3Cloth; + friend class b3Particle; + friend class b3ClothContactManager; + friend class b3ClothSolver; + friend class b3ClothContactSolver; + friend struct b3ParticleBodyContactWorldPoint; + + b3ParticleBodyContact() { } + ~b3ParticleBodyContact() { } + + void Update(); + + b3Particle* m_p1; + b3Shape* m_s2; + + bool m_active; + + // Contact constraint + b3Vec3 m_normal1; + b3Vec3 m_localPoint1; + b3Vec3 m_localPoint2; + float32 m_normalImpulse; + + // Friction constraint + b3Vec3 m_tangent1, m_tangent2; + b3Vec2 m_tangentImpulse; + + b3ParticleBodyContact* m_prev; + b3ParticleBodyContact* m_next; +}; + +struct b3ParticleBodyContactWorldPoint +{ + void Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + + b3Vec3 point; + b3Vec3 normal; + float32 separation; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_contact.h b/include/bounce/cloth/cloth_particle_triangle_contact.h similarity index 94% rename from include/bounce/cloth/cloth_contact.h rename to include/bounce/cloth/cloth_particle_triangle_contact.h index 6e7e291..a5cc347 100644 --- a/include/bounce/cloth/cloth_contact.h +++ b/include/bounce/cloth/cloth_particle_triangle_contact.h @@ -16,8 +16,8 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B3_CLOTH_CONTACT_H -#define B3_CLOTH_CONTACT_H +#ifndef B3_CLOTH_PARTICLE_TRIANGLE_CONTACT_H +#define B3_CLOTH_PARTICLE_TRIANGLE_CONTACT_H #include @@ -29,11 +29,11 @@ class b3ParticleTriangleContact { public: private: + friend class b3List2; friend class b3Cloth; friend class b3Particle; friend class b3ClothTriangle; friend class b3ClothContactManager; - friend class b3List2; friend class b3ClothContactSolver; b3ParticleTriangleContact() { } diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h index 2edf011..75636f6 100644 --- a/include/bounce/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -26,7 +26,7 @@ class b3StackAllocator; class b3Particle; class b3Force; -struct b3ParticleBodyContact; +class b3ParticleBodyContact; class b3ParticleTriangleContact; struct b3ClothSolverDef diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h index ca51ccc..1a1e6db 100644 --- a/include/bounce/cloth/cloth_triangle.h +++ b/include/bounce/cloth/cloth_triangle.h @@ -19,7 +19,7 @@ #ifndef B3_CLOTH_TRIANGLE_H #define B3_CLOTH_TRIANGLE_H -#include +#include // A cloth triangle class b3ClothTriangle diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index 6844e28..225b280 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -19,14 +19,10 @@ #ifndef B3_PARTICLE_H #define B3_PARTICLE_H -#include -#include -#include +#include #include -class b3Shape; class b3Cloth; -class b3Particle; // Static particle: Can be moved manually. // Kinematic particle: Non-zero velocity, can be moved by the solver. @@ -38,7 +34,7 @@ enum b3ParticleType e_dynamicParticle }; -// +// Particle definition struct b3ParticleDef { b3ParticleDef() @@ -63,48 +59,6 @@ struct b3ParticleDef void* userData; }; -// A contact between a particle and a solid -struct b3ParticleBodyContact -{ - b3Particle* p1; - b3Shape* s2; - - // Contact constraint - b3Vec3 normal1; - b3Vec3 localPoint1; - b3Vec3 localPoint2; - float32 normalImpulse; - - // Friction constraint - b3Vec3 t1, t2; - b3Vec2 tangentImpulse; - - bool active; -}; - -struct b3ParticleBodyContactWorldPoint -{ - void Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; - -// Cloth primitive type -enum b3ClothAABBProxyType -{ - e_particleProxy, - e_triangleProxy -}; - -// Cloth primitive broadphase proxy -struct b3ClothAABBProxy -{ - b3ClothAABBProxyType type; - void* owner; -}; - // A cloth particle. class b3Particle { @@ -162,6 +116,7 @@ private: friend class b3ClothSolver; friend class b3ClothForceSolver; friend class b3ClothContactManager; + friend class b3ParticleBodyContact; friend class b3ParticleTriangleContact; friend class b3ClothContactSolver; friend class b3Force; @@ -223,9 +178,6 @@ private: // Parent cloth b3Cloth* m_cloth; - // Contact - b3ParticleBodyContact m_bodyContact; - // AABB Proxy b3ClothAABBProxy m_aabbProxy; diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index a11619c..b397f58 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -140,6 +140,9 @@ public: // Set the user data associated with this shape. void SetUserData(void* data); + // Get broadphase AABB + const b3AABB3& GetAABB() const; + // Dump this shape to the log file. void Dump(u32 bodyIndex) const; diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 8cac18f..0152bac 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -23,14 +23,6 @@ #include #include #include - -#include -#include -#include -#include - -#include - #include static B3_FORCE_INLINE u32 b3NextIndex(u32 i) @@ -501,105 +493,6 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 return b3RayCast(output, input, v1, v2, v3); } -class b3ClothUpdateContactsQueryListener : public b3QueryListener -{ -public: - bool ReportShape(b3Shape* shape) - { - b3Body* body = shape->GetBody(); - - if (body->GetType() != e_staticBody) - { - return true; - } - - b3Transform xf = body->GetTransform(); - - b3TestSphereOutput output; - if (shape->TestSphere(&output, sphere, xf)) - { - if (output.separation < bestSeparation) - { - bestShape = shape; - bestSeparation = output.separation; - bestPoint = output.point; - bestNormal = output.normal; - } - } - - return true; - } - - b3Sphere sphere; - b3Shape* bestShape; - float32 bestSeparation; - b3Vec3 bestPoint; - b3Vec3 bestNormal; -}; - -void b3Cloth::UpdateParticleBodyContacts() -{ - B3_PROFILE("Cloth Update Particle Body Contacts"); - - // Is there a world attached to this cloth? - if (m_world == nullptr) - { - return; - } - - // Create contacts - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - if (p->m_type != e_dynamicParticle) - { - continue; - } - - b3AABB3 aabb = m_contactManager.m_broadPhase.GetAABB(p->m_broadPhaseId); - - b3ClothUpdateContactsQueryListener listener; - listener.sphere.vertex = p->m_position; - listener.sphere.radius = p->m_radius; - listener.bestShape = nullptr; - listener.bestSeparation = 0.0f; - - m_world->QueryAABB(&listener, aabb); - - if (listener.bestShape == nullptr) - { - p->m_bodyContact.active = false; - continue; - } - - b3Shape* shape = listener.bestShape; - b3Body* body = shape->GetBody(); - float32 separation = listener.bestSeparation; - b3Vec3 point = listener.bestPoint; - b3Vec3 normal = -listener.bestNormal; - - b3ParticleBodyContact* c = &p->m_bodyContact; - - b3ParticleBodyContact c0 = *c; - - c->active = true; - c->p1 = p; - c->s2 = shape; - c->normal1 = normal; - c->localPoint1.SetZero(); - c->localPoint2 = body->GetLocalPoint(point); - c->t1 = b3Perp(normal); - c->t2 = b3Cross(c->t1, normal); - c->normalImpulse = 0.0f; - c->tangentImpulse.SetZero(); - - if (c0.active == true) - { - c->normalImpulse = c0.normalImpulse; - c->tangentImpulse = c0.tangentImpulse; - } - } -} - void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Solve"); @@ -624,14 +517,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u solver.Add(f); } - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - if (p->m_bodyContact.active) - { - solver.Add(&p->m_bodyContact); - } - } - for (b3ParticleTriangleContact* c = m_contactManager.m_particleTriangleContactList.m_head; c; c = c->m_next) { if (c->m_active) @@ -640,6 +525,14 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u } } + for (b3ParticleBodyContact* c = m_contactManager.m_particleBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_active) + { + solver.Add(c); + } + } + // Solve solver.Solve(dt, gravity, velocityIterations, positionIterations); } @@ -648,10 +541,7 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Step"); - // Update particle-body contacts - UpdateParticleBodyContacts(); - - // Update self-contacts + // Update contacts m_contactManager.UpdateContacts(); // Integrate state, solve constraints. @@ -695,7 +585,7 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) m_triangles[i].Synchronize(displacement); } - // Find new self-contacts + // Find new contacts m_contactManager.FindNewContacts(); } diff --git a/src/bounce/cloth/cloth_contact_manager.cpp b/src/bounce/cloth/cloth_contact_manager.cpp index d41b4a8..86731c9 100644 --- a/src/bounce/cloth/cloth_contact_manager.cpp +++ b/src/bounce/cloth/cloth_contact_manager.cpp @@ -21,20 +21,107 @@ #include #include #include +#include +#include +#include +#include b3ClothContactManager::b3ClothContactManager() : - m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact)) + m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact)), + m_particleBodyContactBlocks(sizeof(b3ParticleBodyContact)) { } void b3ClothContactManager::FindNewContacts() { - B3_PROFILE("Cloth Find New Contacts"); + FindNewClothContacts(); + FindNewBodyContacts(); +} + +void b3ClothContactManager::FindNewClothContacts() +{ + B3_PROFILE("Cloth Find New Cloth Contacts"); m_broadPhase.FindPairs(this); } +class b3ClothContactManagerFindNewBodyContactsQueryListener : public b3QueryListener +{ +public: + virtual bool ReportShape(b3Shape* s2) + { + cm->AddPSPair(p1, s2); + + // Keep looking for overlaps + return true; + } + + b3ClothContactManager* cm; + b3Particle* p1; +}; + +void b3ClothContactManager::FindNewBodyContacts() +{ + B3_PROFILE("Cloth Find New Body Contacts"); + + // Is there a world attached to this cloth? + if (m_cloth->m_world == nullptr) + { + return; + } + + for (b3Particle* p = m_cloth->m_particleList.m_head; p; p = p->m_next) + { + if (p->m_type != e_dynamicParticle) + { + continue; + } + + b3AABB3 aabb = m_broadPhase.GetAABB(p->m_broadPhaseId); + + b3ClothContactManagerFindNewBodyContactsQueryListener listener; + listener.cm = this; + listener.p1 = p; + + m_cloth->m_world->QueryAABB(&listener, aabb); + } +} + +void b3ClothContactManager::AddPSPair(b3Particle* p1, b3Shape* s2) +{ + // Check if there is a contact between the two entities. + for (b3ParticleBodyContact* c = m_particleBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_p1 == p1 && c->m_s2 == s2) + { + // A contact already exists. + return; + } + } + + bool isntDynamic1 = p1->m_type != e_dynamicParticle; + bool isntDynamic2 = s2->GetBody()->GetType() != e_dynamicBody; + + if (isntDynamic1 && isntDynamic2) + { + // The entities must not collide with each other. + return; + } + + // Create a new contact. + b3ParticleBodyContact* c = CreateParticleBodyContact(); + + c->m_p1 = p1; + c->m_s2 = s2; + c->m_active = false; + c->m_normalImpulse = 0.0f; + c->m_tangentImpulse.SetZero(); + + // Add the contact to the body contact list. + m_particleBodyContactList.PushFront(c); +} + void b3ClothContactManager::AddPair(void* data1, void* data2) { b3ClothAABBProxy* proxy1 = (b3ClothAABBProxy*)data1; @@ -126,9 +213,30 @@ void b3ClothContactManager::Destroy(b3ParticleTriangleContact* c) m_particleTriangleContactBlocks.Free(c); } +b3ParticleBodyContact* b3ClothContactManager::CreateParticleBodyContact() +{ + void* block = m_particleBodyContactBlocks.Allocate(); + return new(block) b3ParticleBodyContact(); +} + +void b3ClothContactManager::Destroy(b3ParticleBodyContact* c) +{ + m_particleBodyContactList.Remove(c); + + c->~b3ParticleBodyContact(); + + m_particleBodyContactBlocks.Free(c); +} + void b3ClothContactManager::UpdateContacts() { - B3_PROFILE("Cloth Update Contacts"); + UpdateClothContacts(); + UpdateBodyContacts(); +} + +void b3ClothContactManager::UpdateClothContacts() +{ + B3_PROFILE("Cloth Update Cloth Contacts"); // Update the state of particle-triangle contacts. b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; @@ -162,6 +270,46 @@ void b3ClothContactManager::UpdateContacts() // The contact persists. c->Update(); + c = c->m_next; + } +} + +void b3ClothContactManager::UpdateBodyContacts() +{ + B3_PROFILE("Cloth Update Body Contacts"); + + // Update the state of particle-body contacts. + b3ParticleBodyContact* c = m_particleBodyContactList.m_head; + while (c) + { + bool isntDynamic1 = c->m_p1->m_type != e_dynamicParticle; + bool isntDynamic2 = c->m_s2->GetType() != e_dynamicBody; + + // Cease the contact if entities must not collide with each other. + if (isntDynamic1 && isntDynamic2) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + b3AABB3 aabb1 = m_broadPhase.GetAABB(c->m_p1->m_broadPhaseId); + b3AABB3 aabb2 = c->m_s2->GetAABB(); + + // Destroy the contact if entities AABBs are not overlapping. + bool overlap = b3TestOverlap(aabb1, aabb2); + if (overlap == false) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + // The contact persists. + c->Update(); + c = c->m_next; } } \ No newline at end of file diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp index d6ef10c..8825734 100644 --- a/src/bounce/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/cloth_contact_solver.cpp @@ -59,35 +59,35 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - vc->indexA = c->p1->m_solverId; - vc->bodyB = c->s2->GetBody(); + vc->indexA = c->m_p1->m_solverId; + vc->bodyB = c->m_s2->GetBody(); - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + vc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; vc->invMassB = vc->bodyB->GetInverseMass(); vc->invIA.SetZero(); vc->invIB = vc->bodyB->GetWorldInverseInertia(); - vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction()); + vc->friction = b3MixFriction(c->m_p1->m_friction, c->m_s2->GetFriction()); - pc->indexA = c->p1->m_solverId; + pc->indexA = c->m_p1->m_solverId; pc->bodyB = vc->bodyB; - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + pc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; pc->invMassB = vc->bodyB->m_invMass; pc->invIA.SetZero(); pc->invIB = vc->bodyB->m_worldInvI; - pc->radiusA = c->p1->m_radius; - pc->radiusB = c->s2->m_radius; + pc->radiusA = c->m_p1->m_radius; + pc->radiusB = c->m_s2->m_radius; pc->localCenterA.SetZero(); pc->localCenterB = pc->bodyB->m_sweep.localCenter; - pc->normalA = c->normal1; - pc->localPointA = c->localPoint1; - pc->localPointB = c->localPoint2; + pc->normalA = c->m_normal1; + pc->localPointA = c->m_localPoint1; + pc->localPointB = c->m_localPoint2; } for (u32 i = 0; i < m_bodyContactCount; ++i) @@ -126,8 +126,8 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB); vc->normal = wp.normal; - vc->tangent1 = c->t1; - vc->tangent2 = c->t2; + vc->tangent1 = c->m_tangent1; + vc->tangent2 = c->m_tangent2; vc->point = wp.point; b3Vec3 point = vc->point; @@ -138,8 +138,8 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() vc->rA = rA; vc->rB = rB; - vc->normalImpulse = c->normalImpulse; - vc->tangentImpulse = c->tangentImpulse; + vc->normalImpulse = c->m_normalImpulse; + vc->tangentImpulse = c->m_tangentImpulse; { b3Vec3 n = vc->normal; @@ -602,8 +602,8 @@ void b3ClothContactSolver::StoreImpulses() b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; + c->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse = vc->tangentImpulse; } for (u32 i = 0; i < m_triangleContactCount; ++i) diff --git a/src/bounce/cloth/cloth_particle_body_contact.cpp b/src/bounce/cloth/cloth_particle_body_contact.cpp new file mode 100644 index 0000000..54b3ed5 --- /dev/null +++ b/src/bounce/cloth/cloth_particle_body_contact.cpp @@ -0,0 +1,62 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +void b3ParticleBodyContact::Update() +{ + b3Sphere sphere; + sphere.radius = m_p1->m_radius; + sphere.vertex = m_p1->m_position; + + b3Shape* shape = m_s2; + b3Body* body = shape->GetBody(); + b3Transform xf = body->GetTransform(); + + b3TestSphereOutput out; + if (shape->TestSphere(&out, sphere, xf) == false) + { + m_active = false; + return; + } + + m_active = true; + m_normal1 = -out.normal; + m_localPoint1.SetZero(); + m_localPoint2 = body->GetLocalPoint(out.point); + m_tangent1 = b3Perp(m_normal1); + m_tangent2 = b3Cross(m_tangent1, m_normal1); +} + +void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) +{ + b3Vec3 nA = c->m_normal1; + + b3Vec3 cA = xfA * c->m_localPoint1; + b3Vec3 cB = xfB * c->m_localPoint2; + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = b3Dot(cB - cA, nA) - rA - rB; +} diff --git a/src/bounce/cloth/cloth_contact.cpp b/src/bounce/cloth/cloth_particle_triangle_contact.cpp similarity index 98% rename from src/bounce/cloth/cloth_contact.cpp rename to src/bounce/cloth/cloth_particle_triangle_contact.cpp index be57dad..8be1b1b 100644 --- a/src/bounce/cloth/cloth_contact.cpp +++ b/src/bounce/cloth/cloth_particle_triangle_contact.cpp @@ -16,7 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#include #include #include #include diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 5584b29..9cdc4c8 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -167,7 +167,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati // Synchronize bodies for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3Body* body = m_bodyContacts[i]->s2->GetBody(); + b3Body* body = m_bodyContacts[i]->m_s2->GetBody(); body->SynchronizeTransform(); diff --git a/src/bounce/cloth/cloth_triangle.cpp b/src/bounce/cloth/cloth_triangle.cpp index 1fc5477..bd5771a 100644 --- a/src/bounce/cloth/cloth_triangle.cpp +++ b/src/bounce/cloth/cloth_triangle.cpp @@ -18,6 +18,7 @@ #include #include +#include #include void b3ClothTriangle::Synchronize(const b3Vec3& displacement) diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp index 308188c..c1da933 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -21,21 +21,6 @@ #include #include -void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) -{ - b3Vec3 nA = c->normal1; - - b3Vec3 cA = b3Mul(xfA, c->localPoint1); - b3Vec3 cB = b3Mul(xfB, c->localPoint2); - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = b3Dot(cB - cA, nA) - rA - rB; -} - b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) { m_cloth = cloth; @@ -63,7 +48,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_userData = nullptr; m_x.SetZero(); m_vertex = ~0; - m_bodyContact.active = false; } b3Particle::~b3Particle() @@ -99,22 +83,38 @@ void b3Particle::SynchronizeTriangles() void b3Particle::DestroyContacts() { - // Destroy body contacts - m_bodyContact.active = false; - - // Destroy triangle contacts - b3ParticleTriangleContact* c = m_cloth->m_contactManager.m_particleTriangleContactList.m_head; - while (c) { - if (c->m_p1 == this) + // Destroy body contacts + b3ParticleBodyContact* c = m_cloth->m_contactManager.m_particleBodyContactList.m_head; + while (c) { - b3ParticleTriangleContact* quack = c; - c = c->m_next; - m_cloth->m_contactManager.Destroy(quack); - continue; - } + if (c->m_p1 == this) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + m_cloth->m_contactManager.Destroy(quack); + continue; + } - c = c->m_next; + c = c->m_next; + } + } + + { + // Destroy triangle contacts + b3ParticleTriangleContact* c = m_cloth->m_contactManager.m_particleTriangleContactList.m_head; + while (c) + { + if (c->m_p1 == this) + { + b3ParticleTriangleContact* quack = c; + c = c->m_next; + m_cloth->m_contactManager.Destroy(quack); + continue; + } + + c = c->m_next; + } } } @@ -132,13 +132,13 @@ void b3Particle::SetType(b3ParticleType type) { m_velocity.SetZero(); m_translation.SetZero(); - + Synchronize(b3Vec3_zero); SynchronizeTriangles(); } DestroyContacts(); - + // Move the proxy so new contacts can be created. m_cloth->m_contactManager.m_broadPhase.TouchProxy(m_broadPhaseId); } \ No newline at end of file diff --git a/src/bounce/dynamics/shapes/shape.cpp b/src/bounce/dynamics/shapes/shape.cpp index da720b5..077b206 100644 --- a/src/bounce/dynamics/shapes/shape.cpp +++ b/src/bounce/dynamics/shapes/shape.cpp @@ -65,6 +65,11 @@ void b3Shape::DestroyContacts() } } +const b3AABB3& b3Shape::GetAABB() const +{ + return m_body->GetWorld()->m_contactMan.m_broadPhase.GetAABB(m_broadPhaseID); +} + void b3Shape::Dump(u32 bodyIndex) const { switch (m_type)