AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small refactor

Browse Source
This commit is contained in:
Irlan 2019-06-24 11:15:02 -03:00
parent b18dae8f9d
commit 5bb247c79b
11 changed files with 443 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
include/bounce/softbody/softbody.h
View File

@ -19,11 +19,9 @@
#ifndef B3_SOFT_BODY_H
#define B3_SOFT_BODY_H
#include <bounce/common/math/transform.h>
#include <bounce/softbody/softbody_contact_manager.h>
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/collision/broad_phase.h>
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;

48
include/bounce/softbody/softbody_contact_manager.h Normal file
View File

@ -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 <bounce/softbody/softbody_node_body_contact.h>
#include <bounce/collision/broad_phase.h>
#include <bounce/common/memory/block_pool.h>
#include <bounce/common/template/list.h>
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<b3NodeBodyContact> m_nodeBodyContactList;
};
#endif

40
include/bounce/softbody/softbody_node.h
View File

@ -23,37 +23,7 @@
#include <bounce/common/math/vec3.h>
#include <bounce/common/math/transform.h>
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

78
include/bounce/softbody/softbody_node_body_contact.h Normal file
View File

@ -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 <bounce/common/template/list.h>
#include <bounce/common/math/vec2.h>
#include <bounce/common/math/vec3.h>
#include <bounce/common/math/transform.h>
struct b3SoftBodyNode;
class b3Shape;
// A contact between a node and a body
class b3NodeBodyContact
{
public:
private:
friend class b3List2<b3NodeBodyContact>;
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

11
src/bounce/cloth/cloth_solver.cpp
View File

@ -169,6 +169,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
{
b3Body* body = m_bodyContacts[i]->m_s2->GetBody();
if (body->GetType() == e_staticBody)
{
continue;
}
body->SynchronizeTransform();
body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
@ -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);

145
src/bounce/softbody/softbody.cpp
View File

@ -19,18 +19,11 @@
#include <bounce/softbody/softbody.h>
#include <bounce/softbody/softbody_mesh.h>
#include <bounce/softbody/softbody_node.h>
#include <bounce/softbody/softbody_solver.h>
#include <bounce/collision/collision.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/world_listeners.h>
#include <bounce/dynamics/body.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/common/draw.h>
// 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)
@ -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

165
src/bounce/softbody/softbody_contact_manager.cpp Normal file
View File

@ -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 <bounce/softbody/softbody_contact_manager.h>
#include <bounce/softbody/softbody.h>
#include <bounce/softbody/softbody_mesh.h>
#include <bounce/softbody/softbody_node.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/world_listeners.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
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);
}

39
src/bounce/softbody/softbody_contact_solver.cpp
View File

@ -17,6 +17,7 @@
*/
#include <bounce/softbody/softbody_contact_solver.h>
#include <bounce/softbody/softbody_node_body_contact.h>
#include <bounce/softbody/softbody_node.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
@ -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;

35
src/bounce/softbody/softbody_node.cpp
View File

@ -19,25 +19,28 @@
#include <bounce/softbody/softbody_node.h>
#include <bounce/softbody/softbody.h>
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;
}
}

62
src/bounce/softbody/softbody_node_body_contact.cpp Normal file
View File

@ -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 <bounce/softbody/softbody_node_body_contact.h>
#include <bounce/softbody/softbody_node.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
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;
}

10
src/bounce/softbody/softbody_solver.cpp
View File

@ -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);