From 5bb247c79bed3391a919065d548af815c1eed459 Mon Sep 17 00:00:00 2001 From: Irlan Date: Mon, 24 Jun 2019 11:15:02 -0300 Subject: [PATCH] Small refactor --- include/bounce/softbody/softbody.h | 19 +- .../softbody/softbody_contact_manager.h | 48 +++++ include/bounce/softbody/softbody_node.h | 40 +---- .../softbody/softbody_node_body_contact.h | 78 +++++++++ src/bounce/cloth/cloth_solver.cpp | 11 +- src/bounce/softbody/softbody.cpp | 147 ++++------------ .../softbody/softbody_contact_manager.cpp | 165 ++++++++++++++++++ .../softbody/softbody_contact_solver.cpp | 39 +++-- src/bounce/softbody/softbody_node.cpp | 35 ++-- .../softbody/softbody_node_body_contact.cpp | 62 +++++++ src/bounce/softbody/softbody_solver.cpp | 10 +- 11 files changed, 443 insertions(+), 211 deletions(-) create mode 100644 include/bounce/softbody/softbody_contact_manager.h create mode 100644 include/bounce/softbody/softbody_node_body_contact.h create mode 100644 src/bounce/softbody/softbody_contact_manager.cpp create mode 100644 src/bounce/softbody/softbody_node_body_contact.cpp diff --git a/include/bounce/softbody/softbody.h b/include/bounce/softbody/softbody.h index 18508f1..fc28e27 100644 --- a/include/bounce/softbody/softbody.h +++ b/include/bounce/softbody/softbody.h @@ -19,11 +19,9 @@ #ifndef B3_SOFT_BODY_H #define B3_SOFT_BODY_H -#include +#include #include -#include - class b3World; struct b3SoftBodyMesh; @@ -141,16 +139,14 @@ public: // Debug draw the body using the associated mesh. void Draw() const; private: - friend struct b3SoftBodyNode; + friend class b3SoftBodyContactManager; friend class b3SoftBodySolver; friend class b3SoftBodyForceSolver; + friend struct b3SoftBodyNode; // Compute mass of each node. void ComputeMass(); - // Update contacts. - void UpdateContacts(); - // Solve void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); @@ -190,8 +186,8 @@ private: // Soft body triangles b3SoftBodyTriangle* m_triangles; - // Broadphase - b3BroadPhase m_broadPhase; + // Contact manager + b3SoftBodyContactManager m_contactManager; // Attached world b3World* m_world; @@ -207,11 +203,6 @@ inline b3Vec3 b3SoftBody::GetGravity() const return m_gravity; } -inline void b3SoftBody::SetWorld(b3World* world) -{ - m_world = world; -} - inline const b3World* b3SoftBody::GetWorld() const { return m_world; diff --git a/include/bounce/softbody/softbody_contact_manager.h b/include/bounce/softbody/softbody_contact_manager.h new file mode 100644 index 0000000..60c1f01 --- /dev/null +++ b/include/bounce/softbody/softbody_contact_manager.h @@ -0,0 +1,48 @@ +/* +* 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_SOFTBODY_CONTACT_MANAGER_H +#define B3_SOFTBODY_CONTACT_MANAGER_H + +#include +#include +#include +#include + +class b3SoftBody; + +// Contact delegator for b3SoftBody. +class b3SoftBodyContactManager +{ +public: + b3SoftBodyContactManager(); + + void FindNewBodyContacts(); + void AddNSPair(b3SoftBodyNode* n1, b3Shape* s2); + void UpdateBodyContacts(); + + b3NodeBodyContact* CreateNodeBodyContact(); + void Destroy(b3NodeBodyContact* c); + + b3BlockPool m_nodeBodyContactBlocks; + b3SoftBody* m_body; + b3BroadPhase m_broadPhase; + b3List2 m_nodeBodyContactList; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/softbody/softbody_node.h b/include/bounce/softbody/softbody_node.h index 604e70b..8e53062 100644 --- a/include/bounce/softbody/softbody_node.h +++ b/include/bounce/softbody/softbody_node.h @@ -23,37 +23,7 @@ #include #include -class b3Shape; class b3SoftBody; -struct b3SoftBodyNode; - -// A contact between a node and a body -struct b3NodeBodyContact -{ - b3SoftBodyNode* n1; - b3Shape* s2; - - // Contact constraint - b3Vec3 normal1; - b3Vec3 localPoint1; - b3Vec3 localPoint2; - float32 normalImpulse; - - // Friction constraint - b3Vec3 t1, t2; - b3Vec2 tangentImpulse; - - bool active; -}; - -struct b3NodeBodyContactWorldPoint -{ - void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; // Static node: Can be moved manually. // Kinematic node: Non-zero velocity, can be moved by the solver. @@ -117,9 +87,11 @@ public: void ApplyForce(const b3Vec3& force); private: friend class b3SoftBody; + friend class b3SoftBodyContactManager; friend class b3SoftBodySolver; friend class b3SoftBodyForceSolver; friend class b3SoftBodyContactSolver; + friend class b3NodeBodyContact; b3SoftBodyNode() { } @@ -128,6 +100,9 @@ private: // Synchronize node void Synchronize(const b3Vec3& displacement); + // Destroy associated contacts + void DestroyContacts(); + // Type b3SoftBodyNodeType m_type; @@ -161,9 +136,6 @@ private: // Soft body mesh vertex index. u32 m_vertex; - // Node and body contact - b3NodeBodyContact m_bodyContact; - // Broadphase proxy u32 m_broadPhaseId; @@ -187,7 +159,7 @@ inline void b3SoftBodyNode::SetType(b3SoftBodyNodeType type) Synchronize(b3Vec3_zero); } - m_bodyContact.active = false; + DestroyContacts(); } inline b3SoftBodyNodeType b3SoftBodyNode::GetType() const diff --git a/include/bounce/softbody/softbody_node_body_contact.h b/include/bounce/softbody/softbody_node_body_contact.h new file mode 100644 index 0000000..8a8bb5e --- /dev/null +++ b/include/bounce/softbody/softbody_node_body_contact.h @@ -0,0 +1,78 @@ +/* +* 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_SOFT_BODY_NODE_BODY_CONTACT_H +#define B3_SOFT_BODY_NODE_BODY_CONTACT_H + +#include +#include +#include +#include + +struct b3SoftBodyNode; +class b3Shape; + +// A contact between a node and a body +class b3NodeBodyContact +{ +public: +private: + friend class b3List2; + friend class b3SoftBody; + friend class b3SoftBodyContactManager; + friend struct b3SoftBodyNode; + friend class b3SoftBodySolver; + friend class b3SoftBodyContactSolver; + friend struct b3NodeBodyContactWorldPoint; + + b3NodeBodyContact() { } + ~b3NodeBodyContact() { } + + void Update(); + + b3SoftBodyNode* m_n1; + b3Shape* m_s2; + + // Is the contact active? + 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; + + // List pointers into the soft body + b3NodeBodyContact* m_prev; + b3NodeBodyContact* m_next; +}; + +struct b3NodeBodyContactWorldPoint +{ + void Initialize(const b3NodeBodyContact* 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/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 6bf66ab..2cae7b4 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -168,6 +168,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati for (u32 i = 0; i < m_bodyContactCount; ++i) { b3Body* body = m_bodyContacts[i]->m_s2->GetBody(); + + if (body->GetType() == e_staticBody) + { + continue; + } body->SynchronizeTransform(); @@ -180,10 +185,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati // Copy state buffers back to the particles for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles[i]; - - p->m_position = positions[i]; - p->m_velocity = velocities[i]; + m_particles[i]->m_position = positions[i]; + m_particles[i]->m_velocity = velocities[i]; } m_allocator->Free(velocities); diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp index e0267ce..3f78684 100644 --- a/src/bounce/softbody/softbody.cpp +++ b/src/bounce/softbody/softbody.cpp @@ -19,18 +19,11 @@ #include #include #include - #include - #include - -#include -#include -#include -#include - #include +// C = A * B static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN) { B3_ASSERT(AN == BM); @@ -49,6 +42,7 @@ static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float3 } } +// B = A^T static B3_FORCE_INLINE void b3Transpose(float32* B, float32* A, u32 AM, u32 AN) { for (u32 i = 0; i < AM; ++i) @@ -62,7 +56,7 @@ static B3_FORCE_INLINE void b3Transpose(float32* B, float32* A, u32 AM, u32 AN) // Compute the elasticity matrix given Young modulus and Poisson's ratio // This is a 6 x 6 matrix -static B3_FORCE_INLINE void b3ComputeD(float32 out[36], +static B3_FORCE_INLINE void b3ComputeD(float32 out[36], float32 E, float32 nu) { float32 lambda = (nu * E) / ((1 + nu) * (1 - 2 * nu)); @@ -463,6 +457,7 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) m_c_max = def.c_max; m_gravity.SetZero(); m_world = nullptr; + m_contactManager.m_body = this; const b3SoftBodyMesh* m = m_mesh; @@ -484,12 +479,11 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) n->m_friction = 0.0f; n->m_userData = nullptr; n->m_vertex = i; - n->m_bodyContact.active = false; b3AABB3 aabb; aabb.Set(n->m_position, 0.0f); - n->m_broadPhaseId = m_broadPhase.CreateProxy(aabb, n); + n->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, n); } // Compute mass @@ -722,107 +716,6 @@ void b3SoftBody::ComputeMass() } } -class b3SoftBodyUpdateContactsQueryListener : 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 b3SoftBody::UpdateContacts() -{ - B3_PROFILE("Soft Body Update Contacts"); - - // Is there a world attached to this soft body? - if (m_world == nullptr) - { - return; - } - - // Create contacts - for (u32 i = 0; i < m_mesh->vertexCount; ++i) - { - b3SoftBodyNode* n = m_nodes + i; - - if (n->m_type != e_dynamicSoftBodyNode) - { - continue; - } - - b3AABB3 aabb = m_broadPhase.GetAABB(n->m_broadPhaseId); - - b3SoftBodyUpdateContactsQueryListener listener; - listener.sphere.vertex = n->m_position; - listener.sphere.radius = n->m_radius; - listener.bestShape = nullptr; - listener.bestSeparation = 0.0f; - - m_world->QueryAABB(&listener, aabb); - - if (listener.bestShape == nullptr) - { - n->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; - - b3NodeBodyContact* c = &n->m_bodyContact; - - b3NodeBodyContact c0 = *c; - - c->active = true; - c->n1 = n; - 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 b3SoftBody::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Soft Body Solve"); @@ -832,12 +725,12 @@ void b3SoftBody::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations b3SoftBodySolver solver(def); - // Push the active contacts - for (u32 i = 0; i < m_mesh->vertexCount; ++i) + // Push the body contacts + for (b3NodeBodyContact* c = m_contactManager.m_nodeBodyContactList.m_head; c; c = c->m_next) { - if (m_nodes[i].m_bodyContact.active) + if (c->m_active) { - solver.Add(&m_nodes[i].m_bodyContact); + solver.Add(c); } } @@ -849,7 +742,7 @@ void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations B3_PROFILE("Soft Body Step"); // Update contacts - UpdateContacts(); + m_contactManager.UpdateBodyContacts(); // Integrate state, solve constraints. if (dt > 0.0f) @@ -877,6 +770,26 @@ void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations n->Synchronize(displacement); } + + // Find new contacts + m_contactManager.FindNewBodyContacts(); +} + +void b3SoftBody::SetWorld(b3World* world) +{ + if (!world && m_world) + { + // Destroy body contacts + b3NodeBodyContact* c = m_contactManager.m_nodeBodyContactList.m_head; + while (c) + { + b3NodeBodyContact* kaboom = c; + c = c->m_next; + m_contactManager.Destroy(kaboom); + } + } + + m_world = world; } void b3SoftBody::Draw() const diff --git a/src/bounce/softbody/softbody_contact_manager.cpp b/src/bounce/softbody/softbody_contact_manager.cpp new file mode 100644 index 0000000..68bbab4 --- /dev/null +++ b/src/bounce/softbody/softbody_contact_manager.cpp @@ -0,0 +1,165 @@ +/* +* 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 +#include +#include +#include +#include + +b3SoftBodyContactManager::b3SoftBodyContactManager() : + m_nodeBodyContactBlocks(sizeof(b3NodeBodyContact)) +{ + +} + +class b3SoftBodyContactManagerFindNewBodyContactsQueryListener : public b3QueryListener +{ +public: + virtual bool ReportShape(b3Shape* s2) + { + cm->AddNSPair(n1, s2); + + // Keep looking for overlaps + return true; + } + + b3SoftBodyContactManager* cm; + b3SoftBodyNode* n1; +}; + +void b3SoftBodyContactManager::FindNewBodyContacts() +{ + B3_PROFILE("Soft Body Find New Body Contacts"); + + // Is there a world attached to this body? + if (m_body->m_world == nullptr) + { + return; + } + + for (u32 i = 0; i < m_body->m_mesh->vertexCount; ++i) + { + b3SoftBodyNode* n = m_body->m_nodes + i; + + if (n->m_type != e_dynamicSoftBodyNode) + { + continue; + } + + b3AABB3 aabb = m_broadPhase.GetAABB(n->m_broadPhaseId); + + b3SoftBodyContactManagerFindNewBodyContactsQueryListener listener; + listener.cm = this; + listener.n1 = n; + + m_body->m_world->QueryAABB(&listener, aabb); + } +} + +void b3SoftBodyContactManager::AddNSPair(b3SoftBodyNode* n1, b3Shape* s2) +{ + // Check if there is a contact between the two entities. + for (b3NodeBodyContact* c = m_nodeBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_n1 == n1 && c->m_s2 == s2) + { + // A contact already exists. + return; + } + } + + bool isntDynamic1 = n1->m_type != e_dynamicSoftBodyNode; + bool isntDynamic2 = s2->GetBody()->GetType() != e_dynamicBody; + + if (isntDynamic1 && isntDynamic2) + { + // The entities must not collide with each other. + return; + } + + // Create a new contact. + b3NodeBodyContact* c = CreateNodeBodyContact(); + + c->m_n1 = n1; + 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_nodeBodyContactList.PushFront(c); +} + +void b3SoftBodyContactManager::UpdateBodyContacts() +{ + B3_PROFILE("Soft Body Update Body Contacts"); + + // Update the state of node-body contacts. + b3NodeBodyContact* c = m_nodeBodyContactList.m_head; + while (c) + { + bool isntDynamic1 = c->m_n1->m_type != e_dynamicSoftBodyNode; + bool isntDynamic2 = c->m_s2->GetBody()->GetType() != e_dynamicBody; + + // Cease the contact if entities must not collide with each other. + if (isntDynamic1 && isntDynamic2) + { + b3NodeBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + b3AABB3 aabb1 = m_broadPhase.GetAABB(c->m_n1->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) + { + b3NodeBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + // The contact persists. + c->Update(); + + c = c->m_next; + } +} + +b3NodeBodyContact* b3SoftBodyContactManager::CreateNodeBodyContact() +{ + void* block = m_nodeBodyContactBlocks.Allocate(); + return new(block) b3NodeBodyContact(); +} + +void b3SoftBodyContactManager::Destroy(b3NodeBodyContact* c) +{ + m_nodeBodyContactList.Remove(c); + + c->~b3NodeBodyContact(); + + m_nodeBodyContactBlocks.Free(c); +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_contact_solver.cpp b/src/bounce/softbody/softbody_contact_solver.cpp index f680d11..bd69ab8 100644 --- a/src/bounce/softbody/softbody_contact_solver.cpp +++ b/src/bounce/softbody/softbody_contact_solver.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -50,35 +51,35 @@ void b3SoftBodyContactSolver::InitializeBodyContactConstraints() b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - vc->indexA = c->n1->m_vertex; - vc->bodyB = c->s2->GetBody(); + vc->indexA = c->m_n1->m_vertex; + vc->bodyB = c->m_s2->GetBody(); - vc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->n1->m_invMass; + vc->invMassA = c->m_n1->m_type == e_staticSoftBodyNode ? 0.0f : c->m_n1->m_invMass; vc->invMassB = vc->bodyB->GetInverseMass(); vc->invIA.SetZero(); vc->invIB = vc->bodyB->GetWorldInverseInertia(); - vc->friction = b3MixFriction(c->n1->m_friction, c->s2->GetFriction()); + vc->friction = b3MixFriction(c->m_n1->m_friction, c->m_s2->GetFriction()); - pc->indexA = c->n1->m_vertex; + pc->indexA = c->m_n1->m_vertex; pc->bodyB = vc->bodyB; - pc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->n1->m_invMass; + pc->invMassA = c->m_n1->m_type == e_staticSoftBodyNode ? 0.0f : c->m_n1->m_invMass; pc->invMassB = vc->bodyB->m_invMass; pc->invIA.SetZero(); pc->invIB = vc->bodyB->m_worldInvI; - pc->radiusA = c->n1->m_radius; - pc->radiusB = c->s2->m_radius; + pc->radiusA = c->m_n1->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) @@ -117,8 +118,8 @@ void b3SoftBodyContactSolver::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; @@ -129,8 +130,8 @@ void b3SoftBodyContactSolver::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; @@ -306,8 +307,8 @@ void b3SoftBodyContactSolver::StoreImpulses() b3NodeBodyContact* c = m_bodyContacts[i]; b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; + c->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse = vc->tangentImpulse; } } @@ -315,14 +316,14 @@ struct b3SoftBodySolverBodyContactSolverPoint { void Initialize(const b3SoftBodySolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) { + b3Vec3 nA = pc->normalA; + b3Vec3 cA = xfA * pc->localPointA; b3Vec3 cB = xfB * pc->localPointB; float32 rA = pc->radiusA; float32 rB = pc->radiusB; - b3Vec3 nA = pc->normalA; - b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA; diff --git a/src/bounce/softbody/softbody_node.cpp b/src/bounce/softbody/softbody_node.cpp index 45a8eb0..cd7bb55 100644 --- a/src/bounce/softbody/softbody_node.cpp +++ b/src/bounce/softbody/softbody_node.cpp @@ -19,25 +19,28 @@ #include #include -void b3NodeBodyContactWorldPoint::Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) -{ - b3Vec3 nA = c->normal1; - - b3Vec3 cA = xfA * c->localPoint1; - b3Vec3 cB = 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; -} - void b3SoftBodyNode::Synchronize(const b3Vec3& displacement) { b3AABB3 aabb; aabb.Set(m_position, m_radius); - m_body->m_broadPhase.MoveProxy(m_broadPhaseId, aabb, displacement); + m_body->m_contactManager.m_broadPhase.MoveProxy(m_broadPhaseId, aabb, displacement); +} + +void b3SoftBodyNode::DestroyContacts() +{ + // Destroy body contacts + b3NodeBodyContact* c = m_body->m_contactManager.m_nodeBodyContactList.m_head; + while (c) + { + if (c->m_n1 == this) + { + b3NodeBodyContact* quack = c; + c = c->m_next; + m_body->m_contactManager.Destroy(quack); + continue; + } + + c = c->m_next; + } } \ No newline at end of file diff --git a/src/bounce/softbody/softbody_node_body_contact.cpp b/src/bounce/softbody/softbody_node_body_contact.cpp new file mode 100644 index 0000000..9cf6401 --- /dev/null +++ b/src/bounce/softbody/softbody_node_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 b3NodeBodyContact::Update() +{ + b3Sphere sphere; + sphere.radius = m_n1->m_radius; + sphere.vertex = m_n1->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 b3NodeBodyContactWorldPoint::Initialize(const b3NodeBodyContact* 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/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp index 54c369d..c7a25ca 100644 --- a/src/bounce/softbody/softbody_solver.cpp +++ b/src/bounce/softbody/softbody_solver.cpp @@ -126,19 +126,15 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter } // Synchronize bodies - for (u32 i = 0; i < m_mesh->vertexCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3SoftBodyNode* n = m_nodes + i; + b3Body* body = m_bodyContacts[i]->m_s2->GetBody(); - b3NodeBodyContact* c = &n->m_bodyContact; - - if (c->active == false) + if (body->GetType() == e_staticBody) { continue; } - b3Body* body = c->s2->GetBody(); - body->SynchronizeTransform(); body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);