New feature: soft bodies!
This commit is contained in:
Irlan
128
examples/testbed/framework/softbody_dragger.cpp
Normal file
128
examples/testbed/framework/softbody_dragger.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
/*
|
||||
* 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 <testbed/framework/softbody_dragger.h>
|
||||
|
||||
b3SoftBodyDragger::b3SoftBodyDragger(b3Ray3* ray, b3SoftBody* body)
|
||||
{
|
||||
m_ray = ray;
|
||||
m_body = body;
|
||||
m_tetrahedron = nullptr;
|
||||
}
|
||||
|
||||
b3SoftBodyDragger::~b3SoftBodyDragger()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool b3SoftBodyDragger::StartDragging()
|
||||
{
|
||||
B3_ASSERT(IsDragging() == false);
|
||||
|
||||
b3SoftBodyRayCastSingleOutput rayOut;
|
||||
if (m_body->RayCastSingle(&rayOut, m_ray->A(), m_ray->B()) == false)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
m_mesh = m_body->GetMesh();
|
||||
m_tetrahedron = m_mesh->tetrahedrons + rayOut.tetrahedron;
|
||||
m_v1 = m_tetrahedron->v1;
|
||||
m_v2 = m_tetrahedron->v2;
|
||||
m_v3 = m_tetrahedron->v3;
|
||||
m_v4 = m_tetrahedron->v4;
|
||||
m_x = rayOut.fraction;
|
||||
|
||||
b3SoftBodyNode* n1 = m_body->GetVertexNode(m_v1);
|
||||
b3SoftBodyNode* n2 = m_body->GetVertexNode(m_v2);
|
||||
b3SoftBodyNode* n3 = m_body->GetVertexNode(m_v3);
|
||||
b3SoftBodyNode* n4 = m_body->GetVertexNode(m_v4);
|
||||
|
||||
b3Vec3 v1 = n1->GetPosition();
|
||||
b3Vec3 v2 = n2->GetPosition();
|
||||
b3Vec3 v3 = n3->GetPosition();
|
||||
b3Vec3 v4 = n4->GetPosition();
|
||||
|
||||
b3Vec3 B = GetPointB();
|
||||
|
||||
float32 wABCD[5];
|
||||
b3BarycentricCoordinates(wABCD, v1, v2, v3, v4, B);
|
||||
|
||||
if (wABCD[4] > B3_EPSILON)
|
||||
{
|
||||
m_tu = wABCD[0] / wABCD[4];
|
||||
m_tv = wABCD[1] / wABCD[4];
|
||||
m_tw = wABCD[2] / wABCD[4];
|
||||
m_tx = wABCD[3] / wABCD[4];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_tu = m_tv = m_tw = m_tx = 0.0f;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void b3SoftBodyDragger::Drag()
|
||||
{
|
||||
B3_ASSERT(IsDragging() == true);
|
||||
|
||||
b3Vec3 A = GetPointA();
|
||||
b3Vec3 B = GetPointB();
|
||||
|
||||
b3Vec3 dx = B - A;
|
||||
|
||||
const float32 k = 100.0f;
|
||||
|
||||
b3Vec3 f = k * dx;
|
||||
|
||||
b3Vec3 f1 = m_tu * f;
|
||||
b3Vec3 f2 = m_tv * f;
|
||||
b3Vec3 f3 = m_tw * f;
|
||||
b3Vec3 f4 = m_tx * f;
|
||||
|
||||
m_body->GetVertexNode(m_v1)->ApplyForce(f1);
|
||||
m_body->GetVertexNode(m_v2)->ApplyForce(f2);
|
||||
m_body->GetVertexNode(m_v3)->ApplyForce(f3);
|
||||
m_body->GetVertexNode(m_v4)->ApplyForce(f4);
|
||||
}
|
||||
|
||||
void b3SoftBodyDragger::StopDragging()
|
||||
{
|
||||
B3_ASSERT(IsDragging() == true);
|
||||
|
||||
m_tetrahedron = nullptr;
|
||||
}
|
||||
|
||||
b3Vec3 b3SoftBodyDragger::GetPointA() const
|
||||
{
|
||||
B3_ASSERT(IsDragging() == true);
|
||||
|
||||
b3Vec3 A = m_body->GetVertexNode(m_v1)->GetPosition();
|
||||
b3Vec3 B = m_body->GetVertexNode(m_v2)->GetPosition();
|
||||
b3Vec3 C = m_body->GetVertexNode(m_v3)->GetPosition();
|
||||
b3Vec3 D = m_body->GetVertexNode(m_v4)->GetPosition();
|
||||
|
||||
return m_tu * A + m_tv * B + m_tw * C + m_tx * D;
|
||||
}
|
||||
|
||||
b3Vec3 b3SoftBodyDragger::GetPointB() const
|
||||
{
|
||||
B3_ASSERT(IsDragging() == true);
|
||||
return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
|
||||
}
|
||||
61
examples/testbed/framework/softbody_dragger.h
Normal file
61
examples/testbed/framework/softbody_dragger.h
Normal file
@ -0,0 +1,61 @@
|
||||
/*
|
||||
* 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_DRAGGER_H
|
||||
#define B3_SOFTBODY_DRAGGER_H
|
||||
|
||||
#include <bounce/common/geometry.h>
|
||||
#include <bounce/softbody/softbody.h>
|
||||
#include <bounce/softbody/softbody_mesh.h>
|
||||
#include <bounce/softbody/softbody_node.h>
|
||||
|
||||
// A soft body triangle dragger.
|
||||
class b3SoftBodyDragger
|
||||
{
|
||||
public:
|
||||
b3SoftBodyDragger(b3Ray3* ray, b3SoftBody* body);
|
||||
~b3SoftBodyDragger();
|
||||
|
||||
bool IsDragging() const;
|
||||
|
||||
bool StartDragging();
|
||||
|
||||
void Drag();
|
||||
|
||||
void StopDragging();
|
||||
|
||||
b3Vec3 GetPointA() const;
|
||||
|
||||
b3Vec3 GetPointB() const;
|
||||
private:
|
||||
b3Ray3* m_ray;
|
||||
float32 m_x;
|
||||
|
||||
b3SoftBody* m_body;
|
||||
const b3SoftBodyMesh* m_mesh;
|
||||
const b3SoftBodyMeshTetrahedron* m_tetrahedron;
|
||||
u32 m_v1, m_v2, m_v3, m_v4;
|
||||
float32 m_tu, m_tv, m_tw, m_tx;
|
||||
};
|
||||
|
||||
inline bool b3SoftBodyDragger::IsDragging() const
|
||||
{
|
||||
return m_tetrahedron != nullptr;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -65,8 +65,10 @@
|
||||
#include <testbed/tests/particle_types.h>
|
||||
#include <testbed/tests/tension_mapping.h>
|
||||
#include <testbed/tests/self_collision.h>
|
||||
#include <testbed/tests/soft_body.h>
|
||||
#include <testbed/tests/rope_test.h>
|
||||
#include <testbed/tests/softbody.h>
|
||||
#include <testbed/tests/pinned_softbody.h>
|
||||
#include <testbed/tests/smash_softbody.h>
|
||||
|
||||
TestEntry g_tests[] =
|
||||
{
|
||||
@ -118,6 +120,8 @@ TestEntry g_tests[] =
|
||||
{ "Tension Mapping", &TensionMapping::Create },
|
||||
{ "Self-Collision", &SelfCollision::Create },
|
||||
{ "Soft Body", &SoftBody::Create },
|
||||
{ "Pinned Soft Body", &PinnedSoftBody::Create },
|
||||
{ "Smash Soft Body", &SmashSoftBody::Create },
|
||||
{ "Rope", &Rope::Create },
|
||||
{ NULL, NULL }
|
||||
};
|
||||
|
||||
135
examples/testbed/tests/pinned_softbody.h
Normal file
135
examples/testbed/tests/pinned_softbody.h
Normal file
@ -0,0 +1,135 @@
|
||||
/*
|
||||
* 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 PINNED_SOFTBODY_H
|
||||
#define PINNED_SOFTBODY_H
|
||||
|
||||
#include <testbed/framework/softbody_dragger.h>
|
||||
|
||||
class PinnedSoftBody : public Test
|
||||
{
|
||||
public:
|
||||
PinnedSoftBody()
|
||||
{
|
||||
m_mesh.SetAsSphere(5.0f, 0);
|
||||
|
||||
// Create soft body
|
||||
b3SoftBodyDef def;
|
||||
def.mesh = &m_mesh;
|
||||
def.density = 0.2f;
|
||||
def.E = 100.0f;
|
||||
def.nu = 0.33f;
|
||||
|
||||
m_body = new b3SoftBody(def);
|
||||
|
||||
u32 pinIndex = ~0;
|
||||
float32 pinDot = -B3_MAX_FLOAT;
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
float32 dot = b3Dot(m_mesh.vertices[i], b3Vec3_y);
|
||||
if (dot > pinDot)
|
||||
{
|
||||
pinDot = dot;
|
||||
pinIndex = i;
|
||||
}
|
||||
}
|
||||
|
||||
b3SoftBodyNode* pinNode = m_body->GetVertexNode(pinIndex);
|
||||
pinNode->SetType(e_staticSoftBodyNode);
|
||||
|
||||
b3Vec3 gravity(0.0f, -9.8f, 0.0f);
|
||||
m_body->SetGravity(gravity);
|
||||
|
||||
m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
|
||||
}
|
||||
|
||||
~PinnedSoftBody()
|
||||
{
|
||||
delete m_bodyDragger;
|
||||
delete m_body;
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
Test::Step();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
m_bodyDragger->Drag();
|
||||
}
|
||||
|
||||
m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
|
||||
|
||||
m_body->Draw();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
b3Vec3 pA = m_bodyDragger->GetPointA();
|
||||
b3Vec3 pB = m_bodyDragger->GetPointB();
|
||||
|
||||
g_draw->DrawPoint(pA, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawPoint(pB, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawSegment(pA, pB, b3Color_white);
|
||||
}
|
||||
|
||||
extern u32 b3_softBodySolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
|
||||
|
||||
float32 E = m_body->GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
}
|
||||
|
||||
void MouseMove(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseMove(pw);
|
||||
}
|
||||
|
||||
void MouseLeftDown(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftDown(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == false)
|
||||
{
|
||||
m_bodyDragger->StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftUp(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == true)
|
||||
{
|
||||
m_bodyDragger->StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new PinnedSoftBody();
|
||||
}
|
||||
|
||||
b3QSoftBodyMesh m_mesh;
|
||||
|
||||
b3SoftBody* m_body;
|
||||
b3SoftBodyDragger* m_bodyDragger;
|
||||
};
|
||||
|
||||
#endif
|
||||
172
examples/testbed/tests/smash_softbody.h
Normal file
172
examples/testbed/tests/smash_softbody.h
Normal file
@ -0,0 +1,172 @@
|
||||
/*
|
||||
* 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 SMASH_SOFTBODY_H
|
||||
#define SMASH_SOFTBODY_H
|
||||
|
||||
#include <testbed/framework/softbody_dragger.h>
|
||||
|
||||
class SmashSoftBody : public Test
|
||||
{
|
||||
public:
|
||||
SmashSoftBody()
|
||||
{
|
||||
m_mesh.SetAsSphere(2.0f, 1);
|
||||
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
m_mesh.vertices[i].y += 3.0f;
|
||||
}
|
||||
|
||||
// Create soft body
|
||||
b3SoftBodyDef def;
|
||||
def.mesh = &m_mesh;
|
||||
def.density = 0.2f;
|
||||
def.E = 100.0f;
|
||||
def.nu = 0.33f;
|
||||
|
||||
m_body = new b3SoftBody(def);
|
||||
|
||||
b3Vec3 gravity(0.0f, -9.8f, 0.0f);
|
||||
m_body->SetGravity(gravity);
|
||||
m_body->SetWorld(&m_world);
|
||||
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_body->GetVertexNode(i);
|
||||
|
||||
n->SetRadius(0.05f);
|
||||
n->SetFriction(0.2f);
|
||||
}
|
||||
|
||||
// Create ground
|
||||
{
|
||||
b3BodyDef bd;
|
||||
bd.type = e_staticBody;
|
||||
|
||||
b3Body* b = m_world.CreateBody(bd);
|
||||
|
||||
b3HullShape groundShape;
|
||||
groundShape.m_hull = &m_groundHull;
|
||||
|
||||
b3ShapeDef sd;
|
||||
sd.shape = &groundShape;
|
||||
sd.friction = 0.3f;
|
||||
|
||||
b->CreateShape(sd);
|
||||
}
|
||||
|
||||
// Create body
|
||||
{
|
||||
b3BodyDef bd;
|
||||
bd.type = e_dynamicBody;
|
||||
bd.position.y = 10.0f;
|
||||
|
||||
b3Body* b = m_world.CreateBody(bd);
|
||||
|
||||
static b3BoxHull boxHull(5.0f, 1.0f, 5.0f);
|
||||
|
||||
b3HullShape boxShape;
|
||||
boxShape.m_hull = &boxHull;
|
||||
|
||||
b3ShapeDef sd;
|
||||
sd.shape = &boxShape;
|
||||
sd.density = 0.1f;
|
||||
sd.friction = 0.3f;
|
||||
|
||||
b->CreateShape(sd);
|
||||
}
|
||||
|
||||
m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
|
||||
}
|
||||
|
||||
~SmashSoftBody()
|
||||
{
|
||||
delete m_bodyDragger;
|
||||
delete m_body;
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
Test::Step();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
m_bodyDragger->Drag();
|
||||
}
|
||||
|
||||
m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
|
||||
|
||||
m_body->Draw();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
b3Vec3 pA = m_bodyDragger->GetPointA();
|
||||
b3Vec3 pB = m_bodyDragger->GetPointB();
|
||||
|
||||
g_draw->DrawPoint(pA, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawPoint(pB, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawSegment(pA, pB, b3Color_white);
|
||||
}
|
||||
|
||||
extern u32 b3_softBodySolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
|
||||
|
||||
float32 E = m_body->GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
}
|
||||
|
||||
void MouseMove(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseMove(pw);
|
||||
}
|
||||
|
||||
void MouseLeftDown(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftDown(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == false)
|
||||
{
|
||||
m_bodyDragger->StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftUp(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == true)
|
||||
{
|
||||
m_bodyDragger->StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new SmashSoftBody();
|
||||
}
|
||||
|
||||
b3QSoftBodyMesh m_mesh;
|
||||
|
||||
b3SoftBody* m_body;
|
||||
b3SoftBodyDragger* m_bodyDragger;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,293 +0,0 @@
|
||||
/*
|
||||
* 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 SOFT_BODY_H
|
||||
#define SOFT_BODY_H
|
||||
|
||||
#include <bounce/meshgen/sphere_mesh.h>
|
||||
|
||||
struct b3QClothMesh : public b3ClothMesh
|
||||
{
|
||||
b3StackArray<b3Vec3, 256> clothVertices;
|
||||
b3StackArray<b3ClothMeshTriangle, 256> clothTriangles;
|
||||
b3ClothMeshMesh clothMesh;
|
||||
|
||||
b3QClothMesh()
|
||||
{
|
||||
vertices = nullptr;
|
||||
vertexCount = 0;
|
||||
triangles = nullptr;
|
||||
triangleCount = 0;
|
||||
meshCount = 0;
|
||||
meshes = nullptr;
|
||||
sewingLineCount = 0;
|
||||
sewingLines = nullptr;
|
||||
}
|
||||
|
||||
void SetAsSphere(float32 radius = 1.0f)
|
||||
{
|
||||
smMesh mesh;
|
||||
smCreateMesh(mesh, 2);
|
||||
|
||||
clothVertices.Resize(mesh.vertexCount);
|
||||
for (u32 i = 0; i < mesh.vertexCount; ++i)
|
||||
{
|
||||
clothVertices[i] = radius * mesh.vertices[i];
|
||||
}
|
||||
|
||||
clothTriangles.Resize(mesh.indexCount / 3);
|
||||
for (u32 i = 0; i < mesh.indexCount / 3; ++i)
|
||||
{
|
||||
clothTriangles[i].v1 = mesh.indices[3 * i + 0];
|
||||
clothTriangles[i].v2 = mesh.indices[3 * i + 1];
|
||||
clothTriangles[i].v3 = mesh.indices[3 * i + 2];
|
||||
}
|
||||
|
||||
clothMesh.startTriangle = 0;
|
||||
clothMesh.triangleCount = clothTriangles.Count();
|
||||
clothMesh.startVertex = 0;
|
||||
clothMesh.vertexCount = clothVertices.Count();
|
||||
|
||||
vertices = clothVertices.Begin();
|
||||
vertexCount = clothVertices.Count();
|
||||
triangles = clothTriangles.Begin();
|
||||
triangleCount = clothTriangles.Count();
|
||||
meshCount = 1;
|
||||
meshes = &clothMesh;
|
||||
}
|
||||
};
|
||||
|
||||
class SoftBody : public Test
|
||||
{
|
||||
public:
|
||||
SoftBody()
|
||||
{
|
||||
m_mesh.SetAsSphere(2.0f);
|
||||
|
||||
// Translate the cloth mesh upwards
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
m_mesh.vertices[i].y += 10.0f;
|
||||
}
|
||||
|
||||
// Create cloth
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_mesh;
|
||||
def.density = 0.2f;
|
||||
def.structural = 10000.0f;
|
||||
def.bending = 0.0f;
|
||||
def.damping = 0.0f;
|
||||
|
||||
m_cloth = new b3Cloth(def);
|
||||
|
||||
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
|
||||
{
|
||||
p->SetRadius(0.05f);
|
||||
p->SetFriction(0.5f);
|
||||
}
|
||||
|
||||
b3Vec3 gravity(0.0f, -9.8f, 0.0f);
|
||||
|
||||
m_cloth->SetGravity(gravity);
|
||||
|
||||
// Attach a world to the cloth
|
||||
m_cloth->SetWorld(&m_world);
|
||||
|
||||
// Create static shapes
|
||||
{
|
||||
b3BodyDef bd;
|
||||
bd.type = e_staticBody;
|
||||
|
||||
b3Body* b = m_world.CreateBody(bd);
|
||||
|
||||
static b3BoxHull boxHull(10.0f, 1.0f, 10.0f);
|
||||
|
||||
b3HullShape boxShape;
|
||||
boxShape.m_hull = &boxHull;
|
||||
|
||||
b3ShapeDef sd;
|
||||
sd.shape = &boxShape;
|
||||
sd.friction = 0.5f;
|
||||
|
||||
b->CreateShape(sd);
|
||||
}
|
||||
|
||||
m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
|
||||
}
|
||||
|
||||
~SoftBody()
|
||||
{
|
||||
delete m_clothDragger;
|
||||
delete m_cloth;
|
||||
}
|
||||
|
||||
// Compute the volume of soft body
|
||||
float32 ComputeVolume() const
|
||||
{
|
||||
const b3ClothMesh* mesh = m_cloth->GetMesh();
|
||||
|
||||
b3StackArray<b3Vec3, 256> positions;
|
||||
positions.Resize(mesh->vertexCount);
|
||||
|
||||
for (u32 i = 0; i < mesh->vertexCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_cloth->GetVertexParticle(i);
|
||||
positions[i] = p->GetPosition();
|
||||
}
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.Compute(positions.Begin(), positions.Count());
|
||||
|
||||
return aabb.Volume();
|
||||
}
|
||||
|
||||
// Apply pressure forces
|
||||
// Explanation available in the paper
|
||||
// "Pressure Model of Soft Body Simulation"
|
||||
void ApplyPressureForces()
|
||||
{
|
||||
const b3ClothMesh* mesh = m_cloth->GetMesh();
|
||||
|
||||
// Volume in m^3
|
||||
float32 V = ComputeVolume();
|
||||
|
||||
// Inverse volume
|
||||
float32 invV = V > 0.0f ? 1.0f / V : 0.0f;
|
||||
|
||||
// Apply pressure forces on particles
|
||||
for (u32 i = 0; i < m_mesh.triangleCount; ++i)
|
||||
{
|
||||
u32 i1 = m_mesh.triangles[i].v1;
|
||||
u32 i2 = m_mesh.triangles[i].v2;
|
||||
u32 i3 = m_mesh.triangles[i].v3;
|
||||
|
||||
b3Particle* p1 = m_cloth->GetVertexParticle(i1);
|
||||
b3Particle* p2 = m_cloth->GetVertexParticle(i2);
|
||||
b3Particle* p3 = m_cloth->GetVertexParticle(i3);
|
||||
|
||||
b3Vec3 v1 = p1->GetPosition();
|
||||
b3Vec3 v2 = p2->GetPosition();
|
||||
b3Vec3 v3 = p3->GetPosition();
|
||||
|
||||
b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
|
||||
|
||||
// Triangle area
|
||||
float32 A = n.Normalize();
|
||||
A *= 0.5f;
|
||||
|
||||
// Ideal Gas Approximation
|
||||
|
||||
// Number of gas moles
|
||||
const float32 k_n = 1.0f;
|
||||
|
||||
// Ideal Gas Constant [J / K mol]
|
||||
const float32 k_R = 8.31f;
|
||||
|
||||
// Gas temperature in Kelvin
|
||||
const float32 k_T = 100.0f;
|
||||
|
||||
// Pressure in Pascals
|
||||
float32 P = invV * k_n * k_R * k_T;
|
||||
|
||||
// Pressure vector
|
||||
b3Vec3 Pn = P * n;
|
||||
|
||||
// Pressure force
|
||||
b3Vec3 FP = Pn * A;
|
||||
|
||||
const float32 inv3 = 1.0f / 3.0f;
|
||||
|
||||
b3Vec3 f = inv3 * FP;
|
||||
|
||||
// Distribute
|
||||
p1->ApplyForce(f);
|
||||
p2->ApplyForce(f);
|
||||
p3->ApplyForce(f);
|
||||
}
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
Test::Step();
|
||||
|
||||
ApplyPressureForces();
|
||||
|
||||
m_cloth->Step(g_testSettings->inv_hertz);
|
||||
|
||||
m_cloth->Draw();
|
||||
|
||||
if (m_clothDragger->IsDragging())
|
||||
{
|
||||
b3Vec3 pA = m_clothDragger->GetPointA();
|
||||
b3Vec3 pB = m_clothDragger->GetPointB();
|
||||
|
||||
g_draw->DrawPoint(pA, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawPoint(pB, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawSegment(pA, pB, b3Color_white);
|
||||
}
|
||||
|
||||
extern u32 b3_clothSolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
|
||||
|
||||
float32 E = m_cloth->GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
}
|
||||
|
||||
void MouseMove(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseMove(pw);
|
||||
|
||||
if (m_clothDragger->IsDragging() == true)
|
||||
{
|
||||
m_clothDragger->Drag();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftDown(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftDown(pw);
|
||||
|
||||
if (m_clothDragger->IsDragging() == false)
|
||||
{
|
||||
m_clothDragger->StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftUp(pw);
|
||||
|
||||
if (m_clothDragger->IsDragging() == true)
|
||||
{
|
||||
m_clothDragger->StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new SoftBody();
|
||||
}
|
||||
|
||||
b3QClothMesh m_mesh;
|
||||
b3Cloth* m_cloth;
|
||||
b3ClothDragger* m_clothDragger;
|
||||
};
|
||||
|
||||
#endif
|
||||
155
examples/testbed/tests/softbody.h
Normal file
155
examples/testbed/tests/softbody.h
Normal file
@ -0,0 +1,155 @@
|
||||
/*
|
||||
* 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 SOFTBODY_H
|
||||
#define SOFTBODY_H
|
||||
|
||||
#include <testbed/framework/softbody_dragger.h>
|
||||
|
||||
class SoftBody : public Test
|
||||
{
|
||||
public:
|
||||
SoftBody()
|
||||
{
|
||||
m_mesh.SetAsSphere(5.0f, 1);
|
||||
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
m_mesh.vertices[i].y += 10.0f;
|
||||
}
|
||||
|
||||
// Create soft body
|
||||
b3SoftBodyDef def;
|
||||
def.mesh = &m_mesh;
|
||||
def.density = 0.2f;
|
||||
def.E = 100.0f;
|
||||
def.nu = 0.33f;
|
||||
|
||||
m_body = new b3SoftBody(def);
|
||||
|
||||
b3Vec3 gravity(0.0f, -9.8f, 0.0f);
|
||||
m_body->SetGravity(gravity);
|
||||
m_body->SetWorld(&m_world);
|
||||
|
||||
for (u32 i = 0; i < m_mesh.vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_body->GetVertexNode(i);
|
||||
|
||||
n->SetRadius(0.05f);
|
||||
n->SetFriction(0.2f);
|
||||
}
|
||||
|
||||
// Create ground
|
||||
{
|
||||
b3BodyDef bd;
|
||||
bd.type = e_staticBody;
|
||||
|
||||
b3Body* b = m_world.CreateBody(bd);
|
||||
|
||||
m_groundHull.SetAsCylinder(20.0f, 2.0f);
|
||||
|
||||
b3HullShape groundShape;
|
||||
groundShape.m_hull = &m_groundHull;
|
||||
|
||||
b3ShapeDef sd;
|
||||
sd.shape = &groundShape;
|
||||
sd.friction = 0.3f;
|
||||
|
||||
b->CreateShape(sd);
|
||||
}
|
||||
|
||||
m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
|
||||
}
|
||||
|
||||
~SoftBody()
|
||||
{
|
||||
delete m_bodyDragger;
|
||||
delete m_body;
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
Test::Step();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
m_bodyDragger->Drag();
|
||||
}
|
||||
|
||||
m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
|
||||
|
||||
m_body->Draw();
|
||||
|
||||
if (m_bodyDragger->IsDragging())
|
||||
{
|
||||
b3Vec3 pA = m_bodyDragger->GetPointA();
|
||||
b3Vec3 pB = m_bodyDragger->GetPointB();
|
||||
|
||||
g_draw->DrawPoint(pA, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawPoint(pB, 2.0f, b3Color_green);
|
||||
|
||||
g_draw->DrawSegment(pA, pB, b3Color_white);
|
||||
}
|
||||
|
||||
extern u32 b3_softBodySolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
|
||||
|
||||
float32 E = m_body->GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
}
|
||||
|
||||
void MouseMove(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseMove(pw);
|
||||
}
|
||||
|
||||
void MouseLeftDown(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftDown(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == false)
|
||||
{
|
||||
m_bodyDragger->StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const b3Ray3& pw)
|
||||
{
|
||||
Test::MouseLeftUp(pw);
|
||||
|
||||
if (m_bodyDragger->IsDragging() == true)
|
||||
{
|
||||
m_bodyDragger->StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new SoftBody();
|
||||
}
|
||||
|
||||
b3QSoftBodyMesh m_mesh;
|
||||
|
||||
b3SoftBody* m_body;
|
||||
b3SoftBodyDragger* m_bodyDragger;
|
||||
|
||||
b3QHull m_groundHull;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -72,4 +72,8 @@
|
||||
#include <bounce/cloth/garment/garment.h>
|
||||
#include <bounce/cloth/garment/garment_mesh.h>
|
||||
|
||||
#include <bounce/softbody/softbody_mesh.h>
|
||||
#include <bounce/softbody/softbody.h>
|
||||
#include <bounce/softbody/softbody_node.h>
|
||||
|
||||
#endif
|
||||
@ -144,6 +144,32 @@ inline void b3BarycentricCoordinates(float32 out[4],
|
||||
out[3] = out[0] + out[1] + out[2];
|
||||
}
|
||||
|
||||
// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w, x)
|
||||
// with respect to a tetrahedron ABCD.
|
||||
// The last output value is the (positive) divisor.
|
||||
inline void b3BarycentricCoordinates(float32 out[5],
|
||||
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D,
|
||||
const b3Vec3& Q)
|
||||
{
|
||||
b3Vec3 AB = B - A;
|
||||
b3Vec3 AC = C - A;
|
||||
b3Vec3 AD = D - A;
|
||||
|
||||
b3Vec3 QA = A - Q;
|
||||
b3Vec3 QB = B - Q;
|
||||
b3Vec3 QC = C - Q;
|
||||
b3Vec3 QD = D - Q;
|
||||
|
||||
float32 divisor = b3Det(AB, AC, AD);
|
||||
float32 sign = b3Sign(divisor);
|
||||
|
||||
out[0] = sign * b3Det(QB, QC, QD);
|
||||
out[1] = sign * b3Det(QA, QD, QC);
|
||||
out[2] = sign * b3Det(QA, QB, QD);
|
||||
out[3] = sign * b3Det(QA, QC, QB);
|
||||
out[4] = sign * divisor;
|
||||
}
|
||||
|
||||
// Project a point onto a segment AB.
|
||||
inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
|
||||
{
|
||||
|
||||
@ -293,9 +293,7 @@ private:
|
||||
friend class b3MeshContact;
|
||||
friend class b3ContactManager;
|
||||
friend class b3ContactSolver;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothContactSolver;
|
||||
|
||||
|
||||
friend class b3Joint;
|
||||
friend class b3JointManager;
|
||||
friend class b3JointSolver;
|
||||
@ -308,6 +306,13 @@ private:
|
||||
|
||||
friend class b3List2<b3Body>;
|
||||
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothContactSolver;
|
||||
|
||||
friend class b3SoftBody;
|
||||
friend class b3SoftBodySolver;
|
||||
friend class b3SoftBodyContactSolver;
|
||||
|
||||
enum b3BodyFlags
|
||||
{
|
||||
e_awakeFlag = 0x0001,
|
||||
|
||||
193
include/bounce/softbody/softbody.h
Normal file
193
include/bounce/softbody/softbody.h
Normal file
@ -0,0 +1,193 @@
|
||||
/*
|
||||
* 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_H
|
||||
#define B3_SOFT_BODY_H
|
||||
|
||||
#include <bounce/common/math/transform.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
class b3World;
|
||||
|
||||
struct b3SoftBodyMesh;
|
||||
|
||||
class b3SoftBodyNode;
|
||||
|
||||
struct b3RayCastInput;
|
||||
struct b3RayCastOutput;
|
||||
|
||||
struct b3SoftBodyRayCastSingleOutput
|
||||
{
|
||||
u32 tetrahedron;
|
||||
u32 v1, v2, v3;
|
||||
float32 fraction;
|
||||
b3Vec3 normal;
|
||||
};
|
||||
|
||||
// Soft body tetrahedron element
|
||||
struct b3SoftBodyElement
|
||||
{
|
||||
float32 invP[16];
|
||||
b3Mat33 K[16];
|
||||
b3Quat q;
|
||||
};
|
||||
|
||||
// Soft body tetrahedron triangle
|
||||
struct b3SoftBodyTriangle
|
||||
{
|
||||
u32 v1, v2, v3;
|
||||
u32 tetrahedron;
|
||||
};
|
||||
|
||||
// Soft body definition
|
||||
// This requires defining a soft body mesh which is typically bound to a render mesh
|
||||
struct b3SoftBodyDef
|
||||
{
|
||||
b3SoftBodyDef()
|
||||
{
|
||||
mesh = nullptr;
|
||||
density = 0.1f;
|
||||
E = 100.0f;
|
||||
nu = 0.3f;
|
||||
}
|
||||
|
||||
// Soft body mesh
|
||||
const b3SoftBodyMesh* mesh;
|
||||
|
||||
// Density in kg/m^3
|
||||
float32 density;
|
||||
|
||||
// Material Young's modulus in [0, inf]
|
||||
// Units are 1e3N/m^2
|
||||
float32 E;
|
||||
|
||||
// Material Poisson ratio in [0, 0.5]
|
||||
// This is a dimensionless value
|
||||
float32 nu;
|
||||
};
|
||||
|
||||
// A soft body represents a deformable volume as a collection of nodes.
|
||||
class b3SoftBody
|
||||
{
|
||||
public:
|
||||
b3SoftBody(const b3SoftBodyDef& def);
|
||||
~b3SoftBody();
|
||||
|
||||
// Perform a ray cast with the soft body.
|
||||
bool RayCastSingle(b3SoftBodyRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
|
||||
|
||||
// Set the acceleration of gravity.
|
||||
void SetGravity(const b3Vec3& gravity);
|
||||
|
||||
// Get the acceleration of gravity.
|
||||
b3Vec3 GetGravity() const;
|
||||
|
||||
// Attach a world to this cloth.
|
||||
// The cloth will be able to respond to collisions with the bodies in the attached world.
|
||||
void SetWorld(b3World* world);
|
||||
|
||||
// Get the world attached to this cloth.
|
||||
const b3World* GetWorld() const;
|
||||
b3World* GetWorld();
|
||||
|
||||
// Return the soft body mesh proxy.
|
||||
const b3SoftBodyMesh* GetMesh() const;
|
||||
|
||||
// Return the node associated with the given vertex.
|
||||
b3SoftBodyNode* GetVertexNode(u32 i);
|
||||
|
||||
// Return the kinetic (or dynamic) energy in this system.
|
||||
float32 GetEnergy() const;
|
||||
|
||||
// Perform a time step.
|
||||
void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
|
||||
|
||||
// Debug draw the body using the associated mesh.
|
||||
void Draw() const;
|
||||
private:
|
||||
// Compute mass of each node.
|
||||
void ComputeMass();
|
||||
|
||||
// Update contacts.
|
||||
void UpdateContacts();
|
||||
|
||||
// Solve
|
||||
void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
|
||||
|
||||
// Stack allocator
|
||||
b3StackAllocator m_stackAllocator;
|
||||
|
||||
// Gravity acceleration
|
||||
b3Vec3 m_gravity;
|
||||
|
||||
// Proxy mesh
|
||||
const b3SoftBodyMesh* m_mesh;
|
||||
|
||||
// Soft body density
|
||||
float32 m_density;
|
||||
|
||||
// Material Young's modulus
|
||||
float32 m_E;
|
||||
|
||||
// Material poisson ratio
|
||||
float32 m_nu;
|
||||
|
||||
// Soft body nodes
|
||||
b3SoftBodyNode* m_nodes;
|
||||
|
||||
// Soft body elements
|
||||
b3SoftBodyElement* m_elements;
|
||||
|
||||
// Soft body triangles
|
||||
b3SoftBodyTriangle* m_triangles;
|
||||
|
||||
// Attached world
|
||||
b3World* m_world;
|
||||
};
|
||||
|
||||
inline void b3SoftBody::SetGravity(const b3Vec3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
inline b3World* b3SoftBody::GetWorld()
|
||||
{
|
||||
return m_world;
|
||||
}
|
||||
|
||||
inline const b3SoftBodyMesh* b3SoftBody::GetMesh() const
|
||||
{
|
||||
return m_mesh;
|
||||
}
|
||||
|
||||
#endif
|
||||
129
include/bounce/softbody/softbody_contact_solver.h
Normal file
129
include/bounce/softbody/softbody_contact_solver.h
Normal file
@ -0,0 +1,129 @@
|
||||
/*
|
||||
* 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_CONTACT_SOLVER_H
|
||||
#define B3_SOFT_BODY_CONTACT_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/mat22.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3SoftBodyNode;
|
||||
class b3Body;
|
||||
|
||||
class b3NodeBodyContact;
|
||||
|
||||
struct b3DenseVec3;
|
||||
|
||||
struct b3SoftBodySolverBodyContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
};
|
||||
|
||||
struct b3SoftBodySolverBodyContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
float32 radiusA;
|
||||
b3Vec3 localCenterA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
float32 radiusB;
|
||||
b3Vec3 localCenterB;
|
||||
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 normalA;
|
||||
b3Vec3 localPointA;
|
||||
b3Vec3 localPointB;
|
||||
};
|
||||
|
||||
struct b3SoftBodyContactSolverDef
|
||||
{
|
||||
b3StackAllocator* allocator;
|
||||
|
||||
b3DenseVec3* positions;
|
||||
b3DenseVec3* velocities;
|
||||
|
||||
u32 bodyContactCapacity;
|
||||
};
|
||||
|
||||
inline float32 b3MixFriction(float32 u1, float32 u2)
|
||||
{
|
||||
return b3Sqrt(u1 * u2);
|
||||
}
|
||||
|
||||
class b3SoftBodyContactSolver
|
||||
{
|
||||
public:
|
||||
b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def);
|
||||
~b3SoftBodyContactSolver();
|
||||
|
||||
void Add(b3NodeBodyContact* c);
|
||||
|
||||
void InitializeBodyContactConstraints();
|
||||
|
||||
void WarmStart();
|
||||
|
||||
void SolveBodyContactVelocityConstraints();
|
||||
|
||||
void StoreImpulses();
|
||||
|
||||
bool SolveBodyContactPositionConstraints();
|
||||
protected:
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
b3DenseVec3* m_positions;
|
||||
b3DenseVec3* m_velocities;
|
||||
|
||||
u32 m_bodyContactCapacity;
|
||||
u32 m_bodyContactCount;
|
||||
b3NodeBodyContact** m_bodyContacts;
|
||||
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
|
||||
b3SoftBodySolverBodyContactPositionConstraint* m_bodyPositionConstraints;
|
||||
};
|
||||
|
||||
#endif
|
||||
45
include/bounce/softbody/softbody_mesh.h
Normal file
45
include/bounce/softbody/softbody_mesh.h
Normal file
@ -0,0 +1,45 @@
|
||||
/*
|
||||
* 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_MESH_H
|
||||
#define B3_SOFT_BODY_MESH_H
|
||||
|
||||
#include <bounce/common/math/vec3.h>
|
||||
|
||||
struct b3SoftBodyMeshTetrahedron
|
||||
{
|
||||
u32 v1, v2, v3, v4;
|
||||
};
|
||||
|
||||
struct b3SoftBodyMesh
|
||||
{
|
||||
u32 vertexCount;
|
||||
b3Vec3* vertices;
|
||||
u32 tetrahedronCount;
|
||||
b3SoftBodyMeshTetrahedron* tetrahedrons;
|
||||
};
|
||||
|
||||
struct b3QSoftBodyMesh : public b3SoftBodyMesh
|
||||
{
|
||||
b3QSoftBodyMesh();
|
||||
~b3QSoftBodyMesh();
|
||||
|
||||
void SetAsSphere(float32 radius, u32 subdivisions);
|
||||
};
|
||||
|
||||
#endif
|
||||
259
include/bounce/softbody/softbody_node.h
Normal file
259
include/bounce/softbody/softbody_node.h
Normal file
@ -0,0 +1,259 @@
|
||||
/*
|
||||
* 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_H
|
||||
#define B3_SOFT_BODY_NODE_H
|
||||
|
||||
#include <bounce/common/math/vec2.h>
|
||||
#include <bounce/common/math/vec3.h>
|
||||
#include <bounce/common/math/transform.h>
|
||||
|
||||
class b3Shape;
|
||||
class b3SoftBody;
|
||||
class b3SoftBodyNode;
|
||||
|
||||
// A contact between a node and a body
|
||||
class b3NodeBodyContact
|
||||
{
|
||||
public:
|
||||
b3NodeBodyContact() { }
|
||||
~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 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;
|
||||
}
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 normal;
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
// Static node: Can be moved manually.
|
||||
// Dynamic node: Non-zero velocity determined by force, can be moved by the solver.
|
||||
enum b3SoftBodyNodeType
|
||||
{
|
||||
e_staticSoftBodyNode,
|
||||
e_dynamicSoftBodyNode
|
||||
};
|
||||
|
||||
// A soft body node.
|
||||
class b3SoftBodyNode
|
||||
{
|
||||
public:
|
||||
// Set the node type.
|
||||
void SetType(b3SoftBodyNodeType type);
|
||||
|
||||
// Get the node type.
|
||||
b3SoftBodyNodeType GetType() const;
|
||||
|
||||
// Get the vertex index.
|
||||
u32 GetVertex() const;
|
||||
|
||||
// Set the particle position.
|
||||
// If the particle is dynamic changing the position directly might lead
|
||||
// to physically incorrect simulation behaviour.
|
||||
void SetPosition(const b3Vec3& position);
|
||||
|
||||
// Get the particle position.
|
||||
const b3Vec3& GetPosition() const;
|
||||
|
||||
// Set the particle velocity.
|
||||
void SetVelocity(const b3Vec3& velocity);
|
||||
|
||||
// Get the particle velocity.
|
||||
const b3Vec3& GetVelocity() const;
|
||||
|
||||
// Get the particle mass.
|
||||
float32 GetMass() const;
|
||||
|
||||
// Set the particle radius.
|
||||
void SetRadius(float32 radius);
|
||||
|
||||
// Get the particle radius.
|
||||
float32 GetRadius() const;
|
||||
|
||||
// Set the particle coefficient of friction.
|
||||
void SetFriction(float32 friction);
|
||||
|
||||
// Get the particle coefficient of friction.
|
||||
float32 GetFriction() const;
|
||||
|
||||
// Apply a force.
|
||||
void ApplyForce(const b3Vec3& force);
|
||||
private:
|
||||
friend class b3SoftBody;
|
||||
friend class b3SoftBodySolver;
|
||||
friend class b3SoftBodyContactSolver;
|
||||
|
||||
b3SoftBodyNode() { }
|
||||
|
||||
~b3SoftBodyNode() { }
|
||||
|
||||
// Type
|
||||
b3SoftBodyNodeType m_type;
|
||||
|
||||
// Position
|
||||
b3Vec3 m_position;
|
||||
|
||||
// Velocity
|
||||
b3Vec3 m_velocity;
|
||||
|
||||
// Applied external force
|
||||
b3Vec3 m_force;
|
||||
|
||||
// Mass
|
||||
float32 m_mass;
|
||||
|
||||
// Inverse mass
|
||||
float32 m_invMass;
|
||||
|
||||
// Radius
|
||||
float32 m_radius;
|
||||
|
||||
// Coefficient of friction
|
||||
float32 m_friction;
|
||||
|
||||
// User data.
|
||||
void* m_userData;
|
||||
|
||||
// Soft body mesh vertex index.
|
||||
u32 m_vertex;
|
||||
|
||||
// Node and body contact
|
||||
b3NodeBodyContact m_bodyContact;
|
||||
|
||||
// Soft body
|
||||
b3SoftBody* m_body;
|
||||
};
|
||||
|
||||
inline void b3SoftBodyNode::SetType(b3SoftBodyNodeType type)
|
||||
{
|
||||
if (m_type == type)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_type = type;
|
||||
m_force.SetZero();
|
||||
|
||||
if (type == e_staticSoftBodyNode)
|
||||
{
|
||||
m_velocity.SetZero();
|
||||
}
|
||||
|
||||
m_bodyContact.active = false;
|
||||
}
|
||||
|
||||
inline b3SoftBodyNodeType b3SoftBodyNode::GetType() const
|
||||
{
|
||||
return m_type;
|
||||
}
|
||||
|
||||
inline u32 b3SoftBodyNode::GetVertex() const
|
||||
{
|
||||
return m_vertex;
|
||||
}
|
||||
|
||||
inline void b3SoftBodyNode::SetPosition(const b3Vec3& position)
|
||||
{
|
||||
m_position = position;
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3SoftBodyNode::GetPosition() const
|
||||
{
|
||||
return m_position;
|
||||
}
|
||||
|
||||
inline void b3SoftBodyNode::SetVelocity(const b3Vec3& velocity)
|
||||
{
|
||||
if (m_type == e_staticSoftBodyNode)
|
||||
{
|
||||
return;
|
||||
}
|
||||
m_velocity = velocity;
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3SoftBodyNode::GetVelocity() const
|
||||
{
|
||||
return m_velocity;
|
||||
}
|
||||
|
||||
inline float32 b3SoftBodyNode::GetMass() const
|
||||
{
|
||||
return m_mass;
|
||||
}
|
||||
|
||||
inline void b3SoftBodyNode::SetRadius(float32 radius)
|
||||
{
|
||||
m_radius = radius;
|
||||
}
|
||||
|
||||
inline float32 b3SoftBodyNode::GetRadius() const
|
||||
{
|
||||
return m_radius;
|
||||
}
|
||||
|
||||
inline void b3SoftBodyNode::SetFriction(float32 friction)
|
||||
{
|
||||
m_friction = friction;
|
||||
}
|
||||
|
||||
inline float32 b3SoftBodyNode::GetFriction() const
|
||||
{
|
||||
return m_friction;
|
||||
}
|
||||
|
||||
inline void b3SoftBodyNode::ApplyForce(const b3Vec3& force)
|
||||
{
|
||||
if (m_type != e_dynamicSoftBodyNode)
|
||||
{
|
||||
return;
|
||||
}
|
||||
m_force += force;
|
||||
}
|
||||
|
||||
#endif
|
||||
56
include/bounce/softbody/softbody_solver.h
Normal file
56
include/bounce/softbody/softbody_solver.h
Normal file
@ -0,0 +1,56 @@
|
||||
/*
|
||||
* 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_SOLVER_H
|
||||
#define B3_SOFT_BODY_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/mat22.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3SoftBodyMesh;
|
||||
|
||||
class b3SoftBodyNode;
|
||||
struct b3SoftBodyElement;
|
||||
|
||||
class b3NodeBodyContact;
|
||||
|
||||
struct b3SoftBodySolverDef
|
||||
{
|
||||
b3StackAllocator* stack;
|
||||
const b3SoftBodyMesh* mesh;
|
||||
b3SoftBodyNode* nodes;
|
||||
b3SoftBodyElement* elements;
|
||||
};
|
||||
|
||||
class b3SoftBodySolver
|
||||
{
|
||||
public:
|
||||
b3SoftBodySolver(const b3SoftBodySolverDef& def);
|
||||
~b3SoftBodySolver();
|
||||
|
||||
void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
|
||||
private:
|
||||
b3StackAllocator* m_allocator;
|
||||
const b3SoftBodyMesh* m_mesh;
|
||||
b3SoftBodyNode* m_nodes;
|
||||
b3SoftBodyElement* m_elements;
|
||||
};
|
||||
|
||||
#endif
|
||||
322
include/bounce/softbody/sparse_mat33.h
Normal file
322
include/bounce/softbody/sparse_mat33.h
Normal file
@ -0,0 +1,322 @@
|
||||
/*
|
||||
* 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_SPARSE_MAT_33_H
|
||||
#define B3_SPARSE_MAT_33_H
|
||||
|
||||
#include <bounce/common/math/mat33.h>
|
||||
#include <bounce/cloth/diag_mat33.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33.h>
|
||||
|
||||
// A sparse matrix.
|
||||
// Each row is a list of non-zero elements in the row.
|
||||
struct b3SparseMat33
|
||||
{
|
||||
//
|
||||
b3SparseMat33();
|
||||
|
||||
//
|
||||
b3SparseMat33(u32 m);
|
||||
|
||||
//
|
||||
b3SparseMat33(const b3SparseMat33& _m);
|
||||
|
||||
//
|
||||
~b3SparseMat33();
|
||||
|
||||
//
|
||||
b3SparseMat33& operator=(const b3SparseMat33& _m);
|
||||
|
||||
//
|
||||
void Copy(const b3SparseMat33& _m);
|
||||
|
||||
//
|
||||
void Destroy();
|
||||
|
||||
//
|
||||
b3Mat33& operator()(u32 i, u32 j);
|
||||
|
||||
//
|
||||
const b3Mat33& operator()(u32 i, u32 j) const;
|
||||
|
||||
//
|
||||
void operator+=(const b3SparseMat33& m);
|
||||
|
||||
//
|
||||
void operator-=(const b3SparseMat33& m);
|
||||
|
||||
u32 rowCount;
|
||||
b3RowValueList* rows;
|
||||
};
|
||||
|
||||
inline b3SparseMat33::b3SparseMat33()
|
||||
{
|
||||
rowCount = 0;
|
||||
rows = nullptr;
|
||||
}
|
||||
|
||||
inline b3SparseMat33::b3SparseMat33(u32 m)
|
||||
{
|
||||
rowCount = m;
|
||||
rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
|
||||
for (u32 i = 0; i < rowCount; ++i)
|
||||
{
|
||||
new (rows + i)b3RowValueList();
|
||||
}
|
||||
}
|
||||
|
||||
inline b3SparseMat33::b3SparseMat33(const b3SparseMat33& m)
|
||||
{
|
||||
rowCount = m.rowCount;
|
||||
rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
|
||||
for (u32 i = 0; i < rowCount; ++i)
|
||||
{
|
||||
new (rows + i)b3RowValueList();
|
||||
}
|
||||
|
||||
Copy(m);
|
||||
}
|
||||
|
||||
inline b3SparseMat33::~b3SparseMat33()
|
||||
{
|
||||
Destroy();
|
||||
}
|
||||
|
||||
inline void b3SparseMat33::Destroy()
|
||||
{
|
||||
for (u32 i = 0; i < rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* vs = rows + i;
|
||||
|
||||
b3RowValue* v = vs->head;
|
||||
while (v)
|
||||
{
|
||||
b3RowValue* v0 = v->next;
|
||||
b3Free(v);
|
||||
v = v0;
|
||||
}
|
||||
|
||||
vs->~b3RowValueList();
|
||||
}
|
||||
|
||||
b3Free(rows);
|
||||
}
|
||||
|
||||
inline b3SparseMat33& b3SparseMat33::operator=(const b3SparseMat33& _m)
|
||||
{
|
||||
if (_m.rows == rows)
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
|
||||
Destroy();
|
||||
|
||||
rowCount = _m.rowCount;
|
||||
rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
|
||||
for (u32 i = 0; i < rowCount; ++i)
|
||||
{
|
||||
new (rows + i)b3RowValueList();
|
||||
}
|
||||
|
||||
Copy(_m);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void b3SparseMat33::Copy(const b3SparseMat33& _m)
|
||||
{
|
||||
B3_ASSERT(rowCount == _m.rowCount);
|
||||
|
||||
for (u32 i = 0; i < rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* vs1 = _m.rows + i;
|
||||
b3RowValueList* vs2 = rows + i;
|
||||
|
||||
B3_ASSERT(vs2->count == 0);
|
||||
|
||||
for (b3RowValue* v1 = vs1->head; v1; v1 = v1->next)
|
||||
{
|
||||
b3RowValue* v2 = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
|
||||
|
||||
v2->column = v1->column;
|
||||
v2->value = v1->value;
|
||||
|
||||
vs2->PushFront(v2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline const b3Mat33& b3SparseMat33::operator()(u32 i, u32 j) const
|
||||
{
|
||||
B3_ASSERT(i < rowCount);
|
||||
B3_ASSERT(j < rowCount);
|
||||
|
||||
b3RowValueList* vs = rows + i;
|
||||
|
||||
for (b3RowValue* v = vs->head; v; v = v->next)
|
||||
{
|
||||
if (v->column == j)
|
||||
{
|
||||
return v->value;
|
||||
}
|
||||
}
|
||||
|
||||
return b3Mat33_zero;
|
||||
}
|
||||
|
||||
inline b3Mat33& b3SparseMat33::operator()(u32 i, u32 j)
|
||||
{
|
||||
B3_ASSERT(i < rowCount);
|
||||
B3_ASSERT(j < rowCount);
|
||||
|
||||
b3RowValueList* vs = rows + i;
|
||||
|
||||
for (b3RowValue* v = vs->head; v; v = v->next)
|
||||
{
|
||||
if (v->column == j)
|
||||
{
|
||||
return v->value;
|
||||
}
|
||||
}
|
||||
|
||||
b3RowValue* v = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
|
||||
v->column = j;
|
||||
v->value.SetZero();
|
||||
|
||||
vs->PushFront(v);
|
||||
|
||||
return v->value;
|
||||
}
|
||||
|
||||
inline void b3SparseMat33::operator+=(const b3SparseMat33& m)
|
||||
{
|
||||
B3_ASSERT(rowCount == m.rowCount);
|
||||
|
||||
for (u32 i = 0; i < m.rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* mvs = m.rows + i;
|
||||
|
||||
for (b3RowValue* v = mvs->head; v; v = v->next)
|
||||
{
|
||||
u32 j = v->column;
|
||||
|
||||
(*this)(i, j) += v->value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void b3SparseMat33::operator-=(const b3SparseMat33& m)
|
||||
{
|
||||
B3_ASSERT(rowCount == m.rowCount);
|
||||
|
||||
for (u32 i = 0; i < m.rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* mvs = m.rows + i;
|
||||
|
||||
for (b3RowValue* v = mvs->head; v; v = v->next)
|
||||
{
|
||||
u32 j = v->column;
|
||||
|
||||
(*this)(i, j) -= v->value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void b3Add(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
|
||||
{
|
||||
out = a;
|
||||
out += b;
|
||||
}
|
||||
|
||||
inline void b3Sub(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
|
||||
{
|
||||
out = a;
|
||||
out -= b;
|
||||
}
|
||||
|
||||
inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v)
|
||||
{
|
||||
B3_ASSERT(A.rowCount == out.n);
|
||||
|
||||
out.SetZero();
|
||||
|
||||
for (u32 i = 0; i < A.rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* vs = A.rows + i;
|
||||
|
||||
for (b3RowValue* vA = vs->head; vA; vA = vA->next)
|
||||
{
|
||||
u32 j = vA->column;
|
||||
b3Mat33 a = vA->value;
|
||||
|
||||
out[i] += a * v[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void b3Mul(b3SparseMat33& out, float32 s, const b3SparseMat33& B)
|
||||
{
|
||||
B3_ASSERT(out.rowCount == B.rowCount);
|
||||
|
||||
if (s == 0.0f)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
out = B;
|
||||
|
||||
for (u32 i = 0; i < out.rowCount; ++i)
|
||||
{
|
||||
b3RowValueList* vs = out.rows + i;
|
||||
for (b3RowValue* v = vs->head; v; v = v->next)
|
||||
{
|
||||
v->value = s * v->value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline b3SparseMat33 operator+(const b3SparseMat33& A, const b3SparseMat33& B)
|
||||
{
|
||||
b3SparseMat33 result(A.rowCount);
|
||||
b3Add(result, A, B);
|
||||
return result;
|
||||
}
|
||||
|
||||
inline b3SparseMat33 operator-(const b3SparseMat33& A, const b3SparseMat33& B)
|
||||
{
|
||||
b3SparseMat33 result(A.rowCount);
|
||||
b3Sub(result, A, B);
|
||||
return result;
|
||||
}
|
||||
|
||||
inline b3SparseMat33 operator*(float32 A, const b3SparseMat33& B)
|
||||
{
|
||||
b3SparseMat33 result(B.rowCount);
|
||||
b3Mul(result, A, B);
|
||||
return result;
|
||||
}
|
||||
|
||||
inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v)
|
||||
{
|
||||
b3DenseVec3 result(v.n);
|
||||
b3Mul(result, A, v);
|
||||
return result;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -275,9 +275,11 @@ workspace(solution_name)
|
||||
|
||||
examples_src_dir .. "/testbed/framework/body_dragger.h",
|
||||
examples_src_dir .. "/testbed/framework/cloth_dragger.h",
|
||||
examples_src_dir .. "/testbed/framework/softbody_dragger.h",
|
||||
|
||||
examples_src_dir .. "/testbed/framework/body_dragger.cpp",
|
||||
examples_src_dir .. "/testbed/framework/cloth_dragger.cpp",
|
||||
examples_src_dir .. "/testbed/framework/softbody_dragger.cpp",
|
||||
|
||||
examples_inc_dir .. "/testbed/tests/**.h",
|
||||
|
||||
@ -370,7 +372,7 @@ end
|
||||
-- clean
|
||||
newaction
|
||||
{
|
||||
trigger = "clean",
|
||||
trigger = "clean",
|
||||
description = "Clean solution",
|
||||
execute = function ()
|
||||
os.rmdir( "doc" )
|
||||
|
||||
992
src/bounce/softbody/softbody.cpp
Normal file
992
src/bounce/softbody/softbody.cpp
Normal file
@ -0,0 +1,992 @@
|
||||
/*
|
||||
* 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.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/body.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN)
|
||||
{
|
||||
B3_ASSERT(AN == BM);
|
||||
|
||||
for (u32 i = 0; i < AM; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < BN; ++j)
|
||||
{
|
||||
C[i + AM * j] = 0.0f;
|
||||
|
||||
for (u32 k = 0; k < AN; ++k)
|
||||
{
|
||||
C[i + AM * j] += A[i + AM * k] * B[k + BM * j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Transpose(float32* B, float32* A, u32 AM, u32 AN)
|
||||
{
|
||||
for (u32 i = 0; i < AM; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < AN; ++j)
|
||||
{
|
||||
B[j + AN * i] = A[i + AM * j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Inverse4(float32 out[16], float32 m[16])
|
||||
{
|
||||
float32 inv[16];
|
||||
|
||||
inv[0] = m[5] * m[10] * m[15] -
|
||||
m[5] * m[11] * m[14] -
|
||||
m[9] * m[6] * m[15] +
|
||||
m[9] * m[7] * m[14] +
|
||||
m[13] * m[6] * m[11] -
|
||||
m[13] * m[7] * m[10];
|
||||
|
||||
inv[4] = -m[4] * m[10] * m[15] +
|
||||
m[4] * m[11] * m[14] +
|
||||
m[8] * m[6] * m[15] -
|
||||
m[8] * m[7] * m[14] -
|
||||
m[12] * m[6] * m[11] +
|
||||
m[12] * m[7] * m[10];
|
||||
|
||||
inv[8] = m[4] * m[9] * m[15] -
|
||||
m[4] * m[11] * m[13] -
|
||||
m[8] * m[5] * m[15] +
|
||||
m[8] * m[7] * m[13] +
|
||||
m[12] * m[5] * m[11] -
|
||||
m[12] * m[7] * m[9];
|
||||
|
||||
inv[12] = -m[4] * m[9] * m[14] +
|
||||
m[4] * m[10] * m[13] +
|
||||
m[8] * m[5] * m[14] -
|
||||
m[8] * m[6] * m[13] -
|
||||
m[12] * m[5] * m[10] +
|
||||
m[12] * m[6] * m[9];
|
||||
|
||||
inv[1] = -m[1] * m[10] * m[15] +
|
||||
m[1] * m[11] * m[14] +
|
||||
m[9] * m[2] * m[15] -
|
||||
m[9] * m[3] * m[14] -
|
||||
m[13] * m[2] * m[11] +
|
||||
m[13] * m[3] * m[10];
|
||||
|
||||
inv[5] = m[0] * m[10] * m[15] -
|
||||
m[0] * m[11] * m[14] -
|
||||
m[8] * m[2] * m[15] +
|
||||
m[8] * m[3] * m[14] +
|
||||
m[12] * m[2] * m[11] -
|
||||
m[12] * m[3] * m[10];
|
||||
|
||||
inv[9] = -m[0] * m[9] * m[15] +
|
||||
m[0] * m[11] * m[13] +
|
||||
m[8] * m[1] * m[15] -
|
||||
m[8] * m[3] * m[13] -
|
||||
m[12] * m[1] * m[11] +
|
||||
m[12] * m[3] * m[9];
|
||||
|
||||
inv[13] = m[0] * m[9] * m[14] -
|
||||
m[0] * m[10] * m[13] -
|
||||
m[8] * m[1] * m[14] +
|
||||
m[8] * m[2] * m[13] +
|
||||
m[12] * m[1] * m[10] -
|
||||
m[12] * m[2] * m[9];
|
||||
|
||||
inv[2] = m[1] * m[6] * m[15] -
|
||||
m[1] * m[7] * m[14] -
|
||||
m[5] * m[2] * m[15] +
|
||||
m[5] * m[3] * m[14] +
|
||||
m[13] * m[2] * m[7] -
|
||||
m[13] * m[3] * m[6];
|
||||
|
||||
inv[6] = -m[0] * m[6] * m[15] +
|
||||
m[0] * m[7] * m[14] +
|
||||
m[4] * m[2] * m[15] -
|
||||
m[4] * m[3] * m[14] -
|
||||
m[12] * m[2] * m[7] +
|
||||
m[12] * m[3] * m[6];
|
||||
|
||||
inv[10] = m[0] * m[5] * m[15] -
|
||||
m[0] * m[7] * m[13] -
|
||||
m[4] * m[1] * m[15] +
|
||||
m[4] * m[3] * m[13] +
|
||||
m[12] * m[1] * m[7] -
|
||||
m[12] * m[3] * m[5];
|
||||
|
||||
inv[14] = -m[0] * m[5] * m[14] +
|
||||
m[0] * m[6] * m[13] +
|
||||
m[4] * m[1] * m[14] -
|
||||
m[4] * m[2] * m[13] -
|
||||
m[12] * m[1] * m[6] +
|
||||
m[12] * m[2] * m[5];
|
||||
|
||||
inv[3] = -m[1] * m[6] * m[11] +
|
||||
m[1] * m[7] * m[10] +
|
||||
m[5] * m[2] * m[11] -
|
||||
m[5] * m[3] * m[10] -
|
||||
m[9] * m[2] * m[7] +
|
||||
m[9] * m[3] * m[6];
|
||||
|
||||
inv[7] = m[0] * m[6] * m[11] -
|
||||
m[0] * m[7] * m[10] -
|
||||
m[4] * m[2] * m[11] +
|
||||
m[4] * m[3] * m[10] +
|
||||
m[8] * m[2] * m[7] -
|
||||
m[8] * m[3] * m[6];
|
||||
|
||||
inv[11] = -m[0] * m[5] * m[11] +
|
||||
m[0] * m[7] * m[9] +
|
||||
m[4] * m[1] * m[11] -
|
||||
m[4] * m[3] * m[9] -
|
||||
m[8] * m[1] * m[7] +
|
||||
m[8] * m[3] * m[5];
|
||||
|
||||
inv[15] = m[0] * m[5] * m[10] -
|
||||
m[0] * m[6] * m[9] -
|
||||
m[4] * m[1] * m[10] +
|
||||
m[4] * m[2] * m[9] +
|
||||
m[8] * m[1] * m[6] -
|
||||
m[8] * m[2] * m[5];
|
||||
|
||||
float32 det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
||||
|
||||
if (det != 0.0f)
|
||||
{
|
||||
det = 1.0f / det;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < 16; ++i)
|
||||
{
|
||||
out[i] = det * inv[i];
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Lame(float32& lambda, float32& mu, float32 E, float32 nu)
|
||||
{
|
||||
lambda = (nu * E) / ((1 + nu) * (1 - 2 * nu));
|
||||
mu = E / (2 * (1 + nu));
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CreateE(float32 out[36], float32 lambda, float32 mu)
|
||||
{
|
||||
float32 E[36] =
|
||||
{
|
||||
lambda + 2 * mu, lambda, lambda, 0, 0, 0,
|
||||
lambda, lambda + 2 * mu, lambda, 0, 0, 0,
|
||||
lambda, lambda, lambda + 2 * mu, 0, 0, 0,
|
||||
0, 0, 0, mu, 0, 0,
|
||||
0, 0, 0, 0, mu, 0,
|
||||
0, 0, 0, 0, 0, mu
|
||||
};
|
||||
|
||||
for (u32 i = 0; i < 36; ++i)
|
||||
{
|
||||
out[i] = E[i];
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CreateB(float32 out[72], float32 invP[16])
|
||||
{
|
||||
float32 a11 = invP[0];
|
||||
float32 a21 = invP[1];
|
||||
float32 a31 = invP[2];
|
||||
float32 a41 = invP[3];
|
||||
|
||||
float32 a12 = invP[4];
|
||||
float32 a22 = invP[5];
|
||||
float32 a32 = invP[6];
|
||||
float32 a42 = invP[7];
|
||||
|
||||
float32 a13 = invP[8];
|
||||
float32 a23 = invP[9];
|
||||
float32 a33 = invP[10];
|
||||
float32 a43 = invP[11];
|
||||
|
||||
//float32 a14 = invP[12];
|
||||
//float32 a24 = invP[13];
|
||||
//float32 a34 = invP[14];
|
||||
//float32 a44 = invP[15];
|
||||
|
||||
// 6 x 12
|
||||
// a11 0 0 a21 0 0 a31 0 0 a41 0 0
|
||||
// 0 a12 0 0 a22 0 0 a32 0 0 a42 0
|
||||
// 0 0 a13 0 0 a23 0 0 a33 0 0 a43
|
||||
// a12 a11 0 a22 a21 0 a32 a31 0 a42 a41 0
|
||||
// 0 a13 a12 0 a23 a22 0 a33 a32 0 a43 a42
|
||||
// a13 0 a11 a23 0 a21 a33 0 a31 a43 0 a41
|
||||
float32 B[72] =
|
||||
{
|
||||
a11, 0, 0, a12, 0, a13,
|
||||
0, a12, 0, a11, a13, 0,
|
||||
0, 0, a13, 0, a12, a11,
|
||||
a21, 0, 0, a22, 0, a23,
|
||||
0, a22, 0, a21, a23, 0,
|
||||
0, 0, a23, 0, a22, a21,
|
||||
a31, 0, 0, a32, 0, a33,
|
||||
0, a32, 0, a31, a33, 0,
|
||||
0, 0, a33, 0, a32, a31,
|
||||
a41, 0, 0, a42, 0, a43,
|
||||
0, a42, 0, a41, a43, 0,
|
||||
0, 0, a43, 0, a42, a41
|
||||
};
|
||||
|
||||
for (u32 i = 0; i < 72; ++i)
|
||||
{
|
||||
out[i] = B[i];
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3SetK(b3Mat33 K[16], float32 Ke[144])
|
||||
{
|
||||
b3Mat33& k11 = K[0 + 4 * 0];
|
||||
b3Mat33& k12 = K[0 + 4 * 1];
|
||||
b3Mat33& k13 = K[0 + 4 * 2];
|
||||
b3Mat33& k14 = K[0 + 4 * 3];
|
||||
|
||||
b3Mat33& k21 = K[1 + 4 * 0];
|
||||
b3Mat33& k22 = K[1 + 4 * 1];
|
||||
b3Mat33& k23 = K[1 + 4 * 2];
|
||||
b3Mat33& k24 = K[1 + 4 * 3];
|
||||
|
||||
b3Mat33& k31 = K[2 + 4 * 0];
|
||||
b3Mat33& k32 = K[2 + 4 * 1];
|
||||
b3Mat33& k33 = K[2 + 4 * 2];
|
||||
b3Mat33& k34 = K[2 + 4 * 3];
|
||||
|
||||
b3Mat33& k41 = K[3 + 4 * 0];
|
||||
b3Mat33& k42 = K[3 + 4 * 1];
|
||||
b3Mat33& k43 = K[3 + 4 * 2];
|
||||
b3Mat33& k44 = K[3 + 4 * 3];
|
||||
|
||||
// k11
|
||||
// a11 a12 a13
|
||||
// a21 a22 a23
|
||||
// a31 a32 a33
|
||||
k11.x.x = Ke[0 + 12 * 0];
|
||||
k11.x.y = Ke[1 + 12 * 0];
|
||||
k11.x.z = Ke[2 + 12 * 0];
|
||||
|
||||
k11.y.x = Ke[0 + 12 * 1];
|
||||
k11.y.y = Ke[1 + 12 * 1];
|
||||
k11.y.z = Ke[2 + 12 * 1];
|
||||
|
||||
k11.z.x = Ke[0 + 12 * 2];
|
||||
k11.z.y = Ke[1 + 12 * 2];
|
||||
k11.z.z = Ke[2 + 12 * 2];
|
||||
|
||||
// k12
|
||||
// a14 a15 a16
|
||||
// a24 a25 a26
|
||||
// a34 a35 a36
|
||||
k12.x.x = Ke[0 + 12 * 3];
|
||||
k12.x.y = Ke[1 + 12 * 3];
|
||||
k12.x.z = Ke[2 + 12 * 3];
|
||||
|
||||
k12.y.x = Ke[0 + 12 * 4];
|
||||
k12.y.y = Ke[1 + 12 * 4];
|
||||
k12.y.z = Ke[2 + 12 * 4];
|
||||
|
||||
k12.z.x = Ke[0 + 12 * 5];
|
||||
k12.z.y = Ke[1 + 12 * 5];
|
||||
k12.z.z = Ke[2 + 12 * 5];
|
||||
|
||||
// k13
|
||||
// a17 a18 a19
|
||||
// a27 a28 a29
|
||||
// a37 a38 a39
|
||||
k13.x.x = Ke[0 + 12 * 6];
|
||||
k13.x.y = Ke[1 + 12 * 6];
|
||||
k13.x.z = Ke[2 + 12 * 6];
|
||||
|
||||
k13.y.x = Ke[0 + 12 * 7];
|
||||
k13.y.y = Ke[1 + 12 * 7];
|
||||
k13.y.z = Ke[2 + 12 * 7];
|
||||
|
||||
k13.z.x = Ke[0 + 12 * 8];
|
||||
k13.z.y = Ke[1 + 12 * 8];
|
||||
k13.z.z = Ke[2 + 12 * 8];
|
||||
|
||||
// k14
|
||||
// a1_10 a1_11 a1_12
|
||||
// a2_10 a2_11 a2_12
|
||||
// a3_10 a3_11 a3_12
|
||||
k14.x.x = Ke[0 + 12 * 9];
|
||||
k14.x.y = Ke[1 + 12 * 9];
|
||||
k14.x.z = Ke[2 + 12 * 9];
|
||||
|
||||
k14.y.x = Ke[0 + 12 * 10];
|
||||
k14.y.y = Ke[1 + 12 * 10];
|
||||
k14.y.z = Ke[2 + 12 * 10];
|
||||
|
||||
k14.z.x = Ke[0 + 12 * 11];
|
||||
k14.z.y = Ke[1 + 12 * 11];
|
||||
k14.z.z = Ke[2 + 12 * 11];
|
||||
|
||||
// k21
|
||||
// a41 a42 a43
|
||||
// a51 a52 a53
|
||||
// a61 a62 a63
|
||||
k21.x.x = Ke[3 + 12 * 0];
|
||||
k21.x.y = Ke[4 + 12 * 0];
|
||||
k21.x.z = Ke[5 + 12 * 0];
|
||||
|
||||
k21.y.x = Ke[3 + 12 * 1];
|
||||
k21.y.y = Ke[4 + 12 * 1];
|
||||
k21.y.z = Ke[5 + 12 * 1];
|
||||
|
||||
k21.z.x = Ke[3 + 12 * 2];
|
||||
k21.z.y = Ke[4 + 12 * 2];
|
||||
k21.z.z = Ke[5 + 12 * 2];
|
||||
|
||||
// k22
|
||||
// a44 a45 a46
|
||||
// a54 a55 a56
|
||||
// a64 a65 a66
|
||||
k22.x.x = Ke[3 + 12 * 3];
|
||||
k22.x.y = Ke[4 + 12 * 3];
|
||||
k22.x.z = Ke[5 + 12 * 3];
|
||||
|
||||
k22.y.x = Ke[3 + 12 * 4];
|
||||
k22.y.y = Ke[4 + 12 * 4];
|
||||
k22.y.z = Ke[5 + 12 * 4];
|
||||
|
||||
k22.z.x = Ke[3 + 12 * 5];
|
||||
k22.z.y = Ke[4 + 12 * 5];
|
||||
k22.z.z = Ke[5 + 12 * 5];
|
||||
|
||||
// k23
|
||||
// a47 a48 a49
|
||||
// a57 a58 a59
|
||||
// a67 a68 a69
|
||||
k23.x.x = Ke[3 + 12 * 6];
|
||||
k23.x.y = Ke[4 + 12 * 6];
|
||||
k23.x.z = Ke[5 + 12 * 6];
|
||||
|
||||
k23.y.x = Ke[3 + 12 * 7];
|
||||
k23.y.y = Ke[4 + 12 * 7];
|
||||
k23.y.z = Ke[5 + 12 * 7];
|
||||
|
||||
k23.z.x = Ke[3 + 12 * 8];
|
||||
k23.z.y = Ke[4 + 12 * 8];
|
||||
k23.z.z = Ke[5 + 12 * 8];
|
||||
|
||||
// k24
|
||||
// a4_10 a4_11 a4_12
|
||||
// a5_10 a5_11 a5_12
|
||||
// a6_10 a6_11 a6_12
|
||||
k24.x.x = Ke[3 + 12 * 9];
|
||||
k24.x.y = Ke[4 + 12 * 9];
|
||||
k24.x.z = Ke[5 + 12 * 9];
|
||||
|
||||
k24.y.x = Ke[3 + 12 * 10];
|
||||
k24.y.y = Ke[4 + 12 * 10];
|
||||
k24.y.z = Ke[5 + 12 * 10];
|
||||
|
||||
k24.z.x = Ke[3 + 12 * 11];
|
||||
k24.z.y = Ke[4 + 12 * 11];
|
||||
k24.z.z = Ke[5 + 12 * 11];
|
||||
|
||||
// k31
|
||||
// a71 a72 a73
|
||||
// a81 a82 a83
|
||||
// a91 a92 a93
|
||||
k31.x.x = Ke[6 + 12 * 0];
|
||||
k31.x.y = Ke[7 + 12 * 0];
|
||||
k31.x.z = Ke[8 + 12 * 0];
|
||||
|
||||
k31.y.x = Ke[6 + 12 * 1];
|
||||
k31.y.y = Ke[7 + 12 * 1];
|
||||
k31.y.z = Ke[8 + 12 * 1];
|
||||
|
||||
k31.z.x = Ke[6 + 12 * 2];
|
||||
k31.z.y = Ke[7 + 12 * 2];
|
||||
k31.z.z = Ke[8 + 12 * 2];
|
||||
|
||||
// k32
|
||||
// a74 a75 a76
|
||||
// a84 a85 a86
|
||||
// a94 a95 a96
|
||||
k32.x.x = Ke[6 + 12 * 3];
|
||||
k32.x.y = Ke[7 + 12 * 3];
|
||||
k32.x.z = Ke[8 + 12 * 3];
|
||||
|
||||
k32.y.x = Ke[6 + 12 * 4];
|
||||
k32.y.y = Ke[7 + 12 * 4];
|
||||
k32.y.z = Ke[8 + 12 * 4];
|
||||
|
||||
k32.z.x = Ke[6 + 12 * 5];
|
||||
k32.z.y = Ke[7 + 12 * 5];
|
||||
k32.z.z = Ke[8 + 12 * 5];
|
||||
|
||||
// k33
|
||||
// a77 a78 a79
|
||||
// a87 a88 a89
|
||||
// a97 a98 a99
|
||||
k33.x.x = Ke[6 + 12 * 6];
|
||||
k33.x.y = Ke[7 + 12 * 6];
|
||||
k33.x.z = Ke[8 + 12 * 6];
|
||||
|
||||
k33.y.x = Ke[6 + 12 * 7];
|
||||
k33.y.y = Ke[7 + 12 * 7];
|
||||
k33.y.z = Ke[8 + 12 * 7];
|
||||
|
||||
k33.z.x = Ke[6 + 12 * 8];
|
||||
k33.z.y = Ke[7 + 12 * 8];
|
||||
k33.z.z = Ke[8 + 12 * 8];
|
||||
|
||||
// k34
|
||||
// a7_10 a7_11 a7_12
|
||||
// a8_10 a8_11 a8_12
|
||||
// a9_10 a9_11 a9_12
|
||||
k34.x.x = Ke[6 + 12 * 9];
|
||||
k34.x.y = Ke[7 + 12 * 9];
|
||||
k34.x.z = Ke[8 + 12 * 9];
|
||||
|
||||
k34.y.x = Ke[6 + 12 * 10];
|
||||
k34.y.y = Ke[7 + 12 * 10];
|
||||
k34.y.z = Ke[8 + 12 * 10];
|
||||
|
||||
k34.z.x = Ke[6 + 12 * 11];
|
||||
k34.z.y = Ke[7 + 12 * 11];
|
||||
k34.z.z = Ke[8 + 12 * 11];
|
||||
|
||||
// k41
|
||||
// a10_1 a10_2 a10_3
|
||||
// a11_1 a11_2 a11_3
|
||||
// a12_1 a12_2 a12_3
|
||||
k41.x.x = Ke[9 + 12 * 0];
|
||||
k41.x.y = Ke[10 + 12 * 0];
|
||||
k41.x.z = Ke[11 + 12 * 0];
|
||||
|
||||
k41.y.x = Ke[9 + 12 * 1];
|
||||
k41.y.y = Ke[10 + 12 * 1];
|
||||
k41.y.z = Ke[11 + 12 * 1];
|
||||
|
||||
k41.z.x = Ke[9 + 12 * 2];
|
||||
k41.z.y = Ke[10 + 12 * 2];
|
||||
k41.z.z = Ke[11 + 12 * 2];
|
||||
|
||||
// k42
|
||||
// a10_4 a10_5 a10_6
|
||||
// a11_4 a11_5 a11_6
|
||||
// a12_4 a12_5 a12_6
|
||||
k42.x.x = Ke[9 + 12 * 3];
|
||||
k42.x.y = Ke[10 + 12 * 3];
|
||||
k42.x.z = Ke[11 + 12 * 3];
|
||||
|
||||
k42.y.x = Ke[9 + 12 * 4];
|
||||
k42.y.y = Ke[10 + 12 * 4];
|
||||
k42.y.z = Ke[11 + 12 * 4];
|
||||
|
||||
k42.z.x = Ke[9 + 12 * 5];
|
||||
k42.z.y = Ke[10 + 12 * 5];
|
||||
k42.z.z = Ke[11 + 12 * 5];
|
||||
|
||||
// k43
|
||||
// a10_7 a10_8 a10_9
|
||||
// a11_7 a11_8 a11_9
|
||||
// a12_7 a12_8 a12_9
|
||||
k43.x.x = Ke[9 + 12 * 6];
|
||||
k43.x.y = Ke[10 + 12 * 6];
|
||||
k43.x.z = Ke[11 + 12 * 6];
|
||||
|
||||
k43.y.x = Ke[9 + 12 * 7];
|
||||
k43.y.y = Ke[10 + 12 * 7];
|
||||
k43.y.z = Ke[11 + 12 * 7];
|
||||
|
||||
k43.z.x = Ke[9 + 12 * 8];
|
||||
k43.z.y = Ke[10 + 12 * 8];
|
||||
k43.z.z = Ke[11 + 12 * 8];
|
||||
|
||||
// k44
|
||||
// a10_10 a10_11 a10_12
|
||||
// a11_10 a11_11 a11_12
|
||||
// a12_10 a12_11 a12_12
|
||||
k44.x.x = Ke[9 + 12 * 9];
|
||||
k44.x.y = Ke[10 + 12 * 9];
|
||||
k44.x.z = Ke[11 + 12 * 9];
|
||||
|
||||
k44.y.x = Ke[9 + 12 * 10];
|
||||
k44.y.y = Ke[10 + 12 * 10];
|
||||
k44.y.z = Ke[11 + 12 * 10];
|
||||
|
||||
k44.z.x = Ke[9 + 12 * 11];
|
||||
k44.z.y = Ke[10 + 12 * 11];
|
||||
k44.z.z = Ke[11 + 12 * 11];
|
||||
}
|
||||
|
||||
b3SoftBody::b3SoftBody(const b3SoftBodyDef& def)
|
||||
{
|
||||
B3_ASSERT(def.mesh);
|
||||
B3_ASSERT(def.density > 0.0f);
|
||||
|
||||
m_mesh = def.mesh;
|
||||
m_density = def.density;
|
||||
m_E = def.E;
|
||||
m_nu = def.nu;
|
||||
m_gravity.SetZero();
|
||||
m_world = nullptr;
|
||||
|
||||
const b3SoftBodyMesh* m = m_mesh;
|
||||
|
||||
m_nodes = (b3SoftBodyNode*)b3Alloc(m->vertexCount * sizeof(b3SoftBodyNode));
|
||||
|
||||
// Initialize nodes
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
n->m_body = this;
|
||||
n->m_type = e_dynamicSoftBodyNode;
|
||||
n->m_position = m->vertices[i];
|
||||
n->m_velocity.SetZero();
|
||||
n->m_force.SetZero();
|
||||
n->m_mass = 0.0f;
|
||||
n->m_invMass = 0.0f;
|
||||
n->m_radius = 0.0f;
|
||||
n->m_friction = 0.0f;
|
||||
n->m_userData = nullptr;
|
||||
n->m_vertex = i;
|
||||
n->m_bodyContact.active = false;
|
||||
}
|
||||
|
||||
// Compute mass
|
||||
ComputeMass();
|
||||
|
||||
// Initialize elements
|
||||
m_elements = (b3SoftBodyElement*)b3Alloc(m->tetrahedronCount * sizeof(b3SoftBodyElement));
|
||||
for (u32 ei = 0; ei < m->tetrahedronCount; ++ei)
|
||||
{
|
||||
b3SoftBodyMeshTetrahedron* mt = m->tetrahedrons + ei;
|
||||
b3SoftBodyElement* e = m_elements + ei;
|
||||
|
||||
u32 v1 = mt->v1;
|
||||
u32 v2 = mt->v2;
|
||||
u32 v3 = mt->v3;
|
||||
u32 v4 = mt->v4;
|
||||
|
||||
b3Vec3 p1 = m->vertices[v1];
|
||||
b3Vec3 p2 = m->vertices[v2];
|
||||
b3Vec3 p3 = m->vertices[v3];
|
||||
b3Vec3 p4 = m->vertices[v4];
|
||||
|
||||
float32 lambda, mu;
|
||||
b3Lame(lambda, mu, m_E, m_nu);
|
||||
|
||||
// 6 x 6
|
||||
float32 E[36];
|
||||
b3CreateE(E, lambda, mu);
|
||||
|
||||
// 4 x 4
|
||||
float32 P[16] =
|
||||
{
|
||||
p1.x, p1.y, p1.z, 1.0f,
|
||||
p2.x, p2.y, p2.z, 1.0f,
|
||||
p3.x, p3.y, p3.z, 1.0f,
|
||||
p4.x, p4.y, p4.z, 1.0f
|
||||
};
|
||||
|
||||
b3Inverse4(e->invP, P);
|
||||
|
||||
// 6 x 12
|
||||
float32 B[72];
|
||||
b3CreateB(B, e->invP);
|
||||
|
||||
// 6 x 12
|
||||
float32 EB[72];
|
||||
b3Mul(EB, E, 6, 6, B, 6, 12);
|
||||
|
||||
// 12 x 6
|
||||
float32 BT[72];
|
||||
b3Transpose(BT, B, 6, 12);
|
||||
|
||||
float32 V = b3Volume(p1, p2, p3, p4);
|
||||
|
||||
// 12 x 12
|
||||
float32 Ke[144];
|
||||
b3Mul(Ke, BT, 12, 6, EB, 6, 12);
|
||||
for (u32 i = 0; i < 144; ++i)
|
||||
{
|
||||
Ke[i] *= V;
|
||||
}
|
||||
|
||||
b3SetK(e->K, Ke);
|
||||
}
|
||||
|
||||
// Initialize triangles
|
||||
m_triangles = (b3SoftBodyTriangle*)b3Alloc(4 * m_mesh->tetrahedronCount * sizeof(b3SoftBodyTriangle));
|
||||
for (u32 i = 0; i < m_mesh->tetrahedronCount; ++i)
|
||||
{
|
||||
b3SoftBodyMeshTetrahedron* mt = m_mesh->tetrahedrons + i;
|
||||
|
||||
u32 v1 = mt->v1;
|
||||
u32 v2 = mt->v2;
|
||||
u32 v3 = mt->v3;
|
||||
u32 v4 = mt->v4;
|
||||
|
||||
b3SoftBodyTriangle* t1 = m_triangles + 4 * i + 0;
|
||||
b3SoftBodyTriangle* t2 = m_triangles + 4 * i + 1;
|
||||
b3SoftBodyTriangle* t3 = m_triangles + 4 * i + 2;
|
||||
b3SoftBodyTriangle* t4 = m_triangles + 4 * i + 3;
|
||||
|
||||
t1->v1 = v1;
|
||||
t1->v2 = v2;
|
||||
t1->v3 = v3;
|
||||
t1->tetrahedron = i;
|
||||
|
||||
t2->v1 = v1;
|
||||
t2->v2 = v3;
|
||||
t2->v3 = v4;
|
||||
t2->tetrahedron = i;
|
||||
|
||||
t3->v1 = v1;
|
||||
t3->v2 = v4;
|
||||
t3->v3 = v2;
|
||||
t3->tetrahedron = i;
|
||||
|
||||
t4->v1 = v2;
|
||||
t4->v2 = v4;
|
||||
t4->v3 = v3;
|
||||
t4->tetrahedron = i;
|
||||
}
|
||||
}
|
||||
|
||||
b3SoftBody::~b3SoftBody()
|
||||
{
|
||||
b3Free(m_nodes);
|
||||
b3Free(m_elements);
|
||||
b3Free(m_triangles);
|
||||
}
|
||||
|
||||
bool b3SoftBody::RayCastSingle(b3SoftBodyRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
|
||||
{
|
||||
b3RayCastInput input;
|
||||
input.p1 = p1;
|
||||
input.p2 = p2;
|
||||
input.maxFraction = 1.0f;
|
||||
|
||||
u32 triangle = ~0;
|
||||
u32 tetrahedron = ~0;
|
||||
|
||||
b3RayCastOutput output0;
|
||||
output0.fraction = B3_MAX_FLOAT;
|
||||
|
||||
for (u32 i = 0; i < 4 * m_mesh->tetrahedronCount; ++i)
|
||||
{
|
||||
b3SoftBodyTriangle* t = m_triangles + i;
|
||||
|
||||
b3Vec3 v1 = m_nodes[t->v1].m_position;
|
||||
b3Vec3 v2 = m_nodes[t->v2].m_position;
|
||||
b3Vec3 v3 = m_nodes[t->v3].m_position;
|
||||
|
||||
b3RayCastOutput subOutput;
|
||||
if (b3RayCast(&subOutput, &input, v1, v2, v3))
|
||||
{
|
||||
if (subOutput.fraction < output0.fraction)
|
||||
{
|
||||
triangle = i;
|
||||
tetrahedron = t->tetrahedron;
|
||||
output0.fraction = subOutput.fraction;
|
||||
output0.normal = subOutput.normal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (tetrahedron != ~0)
|
||||
{
|
||||
output->tetrahedron = tetrahedron;
|
||||
output->v1 = m_triangles[triangle].v1;
|
||||
output->v2 = m_triangles[triangle].v2;
|
||||
output->v3 = m_triangles[triangle].v3;
|
||||
output->fraction = output0.fraction;
|
||||
output->normal = output0.normal;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
b3SoftBodyNode* b3SoftBody::GetVertexNode(u32 i)
|
||||
{
|
||||
B3_ASSERT(i < m_mesh->vertexCount);
|
||||
return m_nodes + i;
|
||||
}
|
||||
|
||||
float32 b3SoftBody::GetEnergy() const
|
||||
{
|
||||
float32 E = 0.0f;
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
E += n->m_mass * b3Dot(n->m_velocity, n->m_velocity);
|
||||
}
|
||||
return 0.5f * E;
|
||||
}
|
||||
|
||||
void b3SoftBody::ComputeMass()
|
||||
{
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
n->m_mass = 0.0f;
|
||||
n->m_invMass = 0.0f;
|
||||
}
|
||||
|
||||
const float32 inv4 = 1.0f / 4.0f;
|
||||
const float32 rho = m_density;
|
||||
|
||||
for (u32 i = 0; i < m_mesh->tetrahedronCount; ++i)
|
||||
{
|
||||
b3SoftBodyMeshTetrahedron* tetrahedron = m_mesh->tetrahedrons + i;
|
||||
|
||||
b3Vec3 v1 = m_mesh->vertices[tetrahedron->v1];
|
||||
b3Vec3 v2 = m_mesh->vertices[tetrahedron->v2];
|
||||
b3Vec3 v3 = m_mesh->vertices[tetrahedron->v3];
|
||||
b3Vec3 v4 = m_mesh->vertices[tetrahedron->v4];
|
||||
|
||||
float32 volume = b3Volume(v1, v2, v3, v4);
|
||||
B3_ASSERT(volume > 0.0f);
|
||||
|
||||
float32 mass = rho * volume;
|
||||
|
||||
b3SoftBodyNode* n1 = m_nodes + tetrahedron->v1;
|
||||
b3SoftBodyNode* n2 = m_nodes + tetrahedron->v2;
|
||||
b3SoftBodyNode* n3 = m_nodes + tetrahedron->v3;
|
||||
b3SoftBodyNode* n4 = m_nodes + tetrahedron->v4;
|
||||
|
||||
n1->m_mass += inv4 * mass;
|
||||
n2->m_mass += inv4 * mass;
|
||||
n3->m_mass += inv4 * mass;
|
||||
n4->m_mass += inv4 * mass;
|
||||
}
|
||||
|
||||
// Invert
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
B3_ASSERT(n->m_mass > 0.0f);
|
||||
n->m_invMass = 1.0f / n->m_mass;
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBody::UpdateContacts()
|
||||
{
|
||||
B3_PROFILE("Soft Body Update Contacts");
|
||||
|
||||
// Is there a world attached to this cloth?
|
||||
if (m_world == nullptr)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Create contacts
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
b3Sphere s1;
|
||||
s1.vertex = n->m_position;
|
||||
s1.radius = n->m_radius;
|
||||
|
||||
// Find the deepest penetration
|
||||
b3Shape* bestShape = nullptr;
|
||||
float32 bestSeparation = 0.0f;
|
||||
b3Vec3 bestPoint(0.0f, 0.0f, 0.0f);
|
||||
b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
|
||||
|
||||
for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext())
|
||||
{
|
||||
if (n->m_type != e_dynamicSoftBodyNode)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (body->GetType() != e_staticBody)
|
||||
{
|
||||
//continue;
|
||||
}
|
||||
|
||||
b3Transform xf = body->GetTransform();
|
||||
for (b3Shape* shape = body->GetShapeList().m_head; shape; shape = shape->GetNext())
|
||||
{
|
||||
b3TestSphereOutput output;
|
||||
if (shape->TestSphere(&output, s1, xf))
|
||||
{
|
||||
if (output.separation < bestSeparation)
|
||||
{
|
||||
bestShape = shape;
|
||||
bestSeparation = output.separation;
|
||||
bestPoint = output.point;
|
||||
bestNormal = output.normal;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (bestShape == nullptr)
|
||||
{
|
||||
n->m_bodyContact.active = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Shape* shape = bestShape;
|
||||
b3Body* body = shape->GetBody();
|
||||
float32 separation = bestSeparation;
|
||||
b3Vec3 point = bestPoint;
|
||||
b3Vec3 normal = -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");
|
||||
|
||||
b3SoftBodySolverDef def;
|
||||
def.stack = &m_stackAllocator;
|
||||
def.mesh = m_mesh;
|
||||
def.nodes = m_nodes;
|
||||
def.elements = m_elements;
|
||||
|
||||
b3SoftBodySolver solver(def);
|
||||
|
||||
solver.Solve(dt, gravity, velocityIterations, positionIterations);
|
||||
}
|
||||
|
||||
void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
|
||||
{
|
||||
B3_PROFILE("Soft Body Step");
|
||||
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
|
||||
// Integrate state, solve constraints.
|
||||
if (dt > 0.0f)
|
||||
{
|
||||
Solve(dt, m_gravity, velocityIterations, positionIterations);
|
||||
}
|
||||
|
||||
// Clear forces
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
m_nodes[i].m_force.SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBody::Draw() const
|
||||
{
|
||||
const b3SoftBodyMesh* m = m_mesh;
|
||||
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
b3Vec3 v = n->m_position;
|
||||
|
||||
if (n->m_type == e_staticSoftBodyNode)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(v, 4.0f, b3Color_white);
|
||||
}
|
||||
|
||||
if (n->m_type == e_dynamicSoftBodyNode)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(v, 4.0f, b3Color_green);
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m->tetrahedronCount; ++i)
|
||||
{
|
||||
b3SoftBodyMeshTetrahedron* t = m->tetrahedrons + i;
|
||||
|
||||
b3Vec3 v1 = m_nodes[t->v1].m_position;
|
||||
b3Vec3 v2 = m_nodes[t->v2].m_position;
|
||||
b3Vec3 v3 = m_nodes[t->v3].m_position;
|
||||
b3Vec3 v4 = m_nodes[t->v4].m_position;
|
||||
|
||||
b3Vec3 c = (v1 + v2 + v3 + v4) / 4.0f;
|
||||
|
||||
float32 s = 0.9f;
|
||||
|
||||
v1 = s * (v1 - c) + c;
|
||||
v2 = s * (v2 - c) + c;
|
||||
v3 = s * (v3 - c) + c;
|
||||
v4 = s * (v4 - c) + c;
|
||||
|
||||
// v1, v2, v3
|
||||
b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black);
|
||||
|
||||
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
|
||||
n1.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(-n1, v1, v2, v3, b3Color_blue);
|
||||
|
||||
// v1, v3, v4
|
||||
b3Draw_draw->DrawTriangle(v1, v3, v4, b3Color_black);
|
||||
|
||||
b3Vec3 n2 = b3Cross(v3 - v1, v4 - v1);
|
||||
n2.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(-n2, v1, v3, v4, b3Color_blue);
|
||||
|
||||
// v1, v4, v2
|
||||
b3Draw_draw->DrawTriangle(v1, v4, v2, b3Color_black);
|
||||
|
||||
b3Vec3 n3 = b3Cross(v4 - v1, v2 - v1);
|
||||
n3.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(-n3, v1, v4, v2, b3Color_blue);
|
||||
|
||||
// v2, v4, v3
|
||||
b3Draw_draw->DrawTriangle(v2, v4, v3, b3Color_black);
|
||||
|
||||
b3Vec3 n4 = b3Cross(v4 - v2, v3 - v2);
|
||||
n4.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(-n4, v2, v4, v3, b3Color_blue);
|
||||
}
|
||||
}
|
||||
429
src/bounce/softbody/softbody_contact_solver.cpp
Normal file
429
src/bounce/softbody/softbody_contact_solver.cpp
Normal file
@ -0,0 +1,429 @@
|
||||
/*
|
||||
* 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_solver.h>
|
||||
#include <bounce/softbody/softbody_node.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
#include <bounce/dynamics/body.h>
|
||||
|
||||
b3SoftBodyContactSolver::b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def)
|
||||
{
|
||||
m_allocator = def.allocator;
|
||||
|
||||
m_positions = def.positions;
|
||||
m_velocities = def.velocities;
|
||||
|
||||
m_bodyContactCapacity = def.bodyContactCapacity;
|
||||
m_bodyContactCount = 0;
|
||||
m_bodyContacts = (b3NodeBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3NodeBodyContact*));
|
||||
}
|
||||
|
||||
b3SoftBodyContactSolver::~b3SoftBodyContactSolver()
|
||||
{
|
||||
m_allocator->Free(m_bodyPositionConstraints);
|
||||
m_allocator->Free(m_bodyVelocityConstraints);
|
||||
m_allocator->Free(m_bodyContacts);
|
||||
}
|
||||
|
||||
void b3SoftBodyContactSolver::Add(b3NodeBodyContact* c)
|
||||
{
|
||||
m_bodyContacts[m_bodyContactCount++] = c;
|
||||
}
|
||||
|
||||
void b3SoftBodyContactSolver::InitializeBodyContactConstraints()
|
||||
{
|
||||
m_bodyVelocityConstraints = (b3SoftBodySolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactVelocityConstraint));
|
||||
m_bodyPositionConstraints = (b3SoftBodySolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactPositionConstraint));
|
||||
|
||||
b3DenseVec3& x = *m_positions;
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3NodeBodyContact* c = m_bodyContacts[i];
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
vc->indexA = c->n1->m_vertex;
|
||||
vc->bodyB = c->s2->GetBody();
|
||||
|
||||
vc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->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());
|
||||
|
||||
pc->indexA = c->n1->m_vertex;
|
||||
pc->bodyB = vc->bodyB;
|
||||
|
||||
pc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->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->localCenterA.SetZero();
|
||||
pc->localCenterB = pc->bodyB->m_sweep.localCenter;
|
||||
|
||||
pc->normalA = c->normal1;
|
||||
pc->localPointA = c->localPoint1;
|
||||
pc->localPointB = c->localPoint2;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3NodeBodyContact* c = m_bodyContacts[i];
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = bodyB->m_sweep.worldCenter;
|
||||
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
b3Quat qB = bodyB->m_sweep.orientation;
|
||||
|
||||
b3Vec3 localCenterA = pc->localCenterA;
|
||||
b3Vec3 localCenterB = pc->localCenterB;
|
||||
|
||||
b3Transform xfA;
|
||||
xfA.rotation = b3QuatMat33(qA);
|
||||
xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
|
||||
|
||||
b3Transform xfB;
|
||||
xfB.rotation = b3QuatMat33(qB);
|
||||
xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
|
||||
|
||||
b3NodeBodyContactWorldPoint wp;
|
||||
wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB);
|
||||
|
||||
vc->normal = wp.normal;
|
||||
vc->tangent1 = c->t1;
|
||||
vc->tangent2 = c->t2;
|
||||
vc->point = wp.point;
|
||||
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
b3Vec3 rA = point - xA;
|
||||
b3Vec3 rB = point - xB;
|
||||
|
||||
vc->rA = rA;
|
||||
vc->rB = rB;
|
||||
|
||||
vc->normalImpulse = c->normalImpulse;
|
||||
vc->tangentImpulse = c->tangentImpulse;
|
||||
|
||||
{
|
||||
b3Vec3 n = vc->normal;
|
||||
|
||||
b3Vec3 rnA = b3Cross(rA, n);
|
||||
b3Vec3 rnB = b3Cross(rB, n);
|
||||
|
||||
float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
vc->velocityBias = 0.0f;
|
||||
}
|
||||
|
||||
{
|
||||
b3Vec3 t1 = vc->tangent1;
|
||||
b3Vec3 t2 = vc->tangent2;
|
||||
|
||||
b3Vec3 rn1A = b3Cross(rA, t1);
|
||||
b3Vec3 rn1B = b3Cross(rB, t1);
|
||||
b3Vec3 rn2A = b3Cross(rA, t2);
|
||||
b3Vec3 rn2B = b3Cross(rB, t2);
|
||||
|
||||
// dot(t1, t2) = 0
|
||||
// J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0
|
||||
float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B);
|
||||
float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B);
|
||||
float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B);
|
||||
|
||||
b3Mat22 K;
|
||||
K.x.Set(k11, k12);
|
||||
K.y.Set(k12, k22);
|
||||
|
||||
vc->tangentMass = b3Inverse(K);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBodyContactSolver::WarmStart()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 P = vc->normalImpulse * vc->normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(vc->rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(vc->rB, P);
|
||||
|
||||
b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
|
||||
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * b3Cross(vc->rA, P1 + P2);
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * b3Cross(vc->rB, P1 + P2);
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBodyContactSolver::SolveBodyContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 normal = vc->normal;
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
b3Vec3 rA = vc->rA;
|
||||
b3Vec3 rB = vc->rB;
|
||||
|
||||
// Solve normal constraint.
|
||||
{
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
float32 Cdot = b3Dot(normal, dv);
|
||||
|
||||
float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias);
|
||||
|
||||
float32 oldImpulse = vc->normalImpulse;
|
||||
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
|
||||
impulse = vc->normalImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
// Solve tangent constraints.
|
||||
{
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
|
||||
b3Vec2 Cdot;
|
||||
Cdot.x = b3Dot(dv, vc->tangent1);
|
||||
Cdot.y = b3Dot(dv, vc->tangent2);
|
||||
|
||||
b3Vec2 impulse = vc->tangentMass * -Cdot;
|
||||
b3Vec2 oldImpulse = vc->tangentImpulse;
|
||||
vc->tangentImpulse += impulse;
|
||||
|
||||
float32 maxImpulse = vc->friction * vc->normalImpulse;
|
||||
if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse)
|
||||
{
|
||||
vc->tangentImpulse.Normalize();
|
||||
vc->tangentImpulse *= maxImpulse;
|
||||
}
|
||||
|
||||
impulse = vc->tangentImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P1 = impulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = impulse.y * vc->tangent2;
|
||||
b3Vec3 P = P1 + P2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBodyContactSolver::StoreImpulses()
|
||||
{
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3NodeBodyContact* c = m_bodyContacts[i];
|
||||
b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
c->tangentImpulse = vc->tangentImpulse;
|
||||
}
|
||||
}
|
||||
|
||||
struct b3ClothSolverBodyContactSolverPoint
|
||||
{
|
||||
void Initialize(const b3SoftBodySolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
|
||||
{
|
||||
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;
|
||||
|
||||
point = cB;
|
||||
normal = nA;
|
||||
separation = b3Dot(cB - cA, nA) - rA - rB;
|
||||
}
|
||||
|
||||
b3Vec3 normal;
|
||||
b3Vec3 point;
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
bool b3SoftBodyContactSolver::SolveBodyContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
u32 indexA = pc->indexA;
|
||||
float32 mA = pc->invMassA;
|
||||
b3Mat33 iA = pc->invIA;
|
||||
b3Vec3 localCenterA = pc->localCenterA;
|
||||
|
||||
b3Body* bodyB = pc->bodyB;
|
||||
float32 mB = pc->invMassB;
|
||||
b3Mat33 iB = pc->invIB;
|
||||
b3Vec3 localCenterB = pc->localCenterB;
|
||||
|
||||
b3Vec3 cA = x[indexA];
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
|
||||
b3Vec3 cB = bodyB->m_sweep.worldCenter;
|
||||
b3Quat qB = bodyB->m_sweep.orientation;
|
||||
|
||||
// Solve normal constraint
|
||||
b3Transform xfA;
|
||||
xfA.rotation = b3QuatMat33(qA);
|
||||
xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
|
||||
|
||||
b3Transform xfB;
|
||||
xfB.rotation = b3QuatMat33(qB);
|
||||
xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
|
||||
|
||||
b3ClothSolverBodyContactSolverPoint cpcp;
|
||||
cpcp.Initialize(pc, xfA, xfB);
|
||||
|
||||
b3Vec3 normal = cpcp.normal;
|
||||
b3Vec3 point = cpcp.point;
|
||||
float32 separation = cpcp.separation;
|
||||
|
||||
// Update max constraint error.
|
||||
minSeparation = b3Min(minSeparation, separation);
|
||||
|
||||
// Allow some slop and prevent large corrections.
|
||||
float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
|
||||
|
||||
// Compute effective mass.
|
||||
b3Vec3 rA = point - cA;
|
||||
b3Vec3 rB = point - cB;
|
||||
|
||||
b3Vec3 rnA = b3Cross(rA, normal);
|
||||
b3Vec3 rnB = b3Cross(rB, normal);
|
||||
float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB);
|
||||
|
||||
// Compute normal impulse.
|
||||
float32 impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
cA -= mA * P;
|
||||
qA -= b3Derivative(qA, iA * b3Cross(rA, P));
|
||||
qA.Normalize();
|
||||
|
||||
cB += mB * P;
|
||||
qB += b3Derivative(qB, iB * b3Cross(rB, P));
|
||||
qB.Normalize();
|
||||
|
||||
x[indexA] = cA;
|
||||
|
||||
bodyB->m_sweep.worldCenter = cB;
|
||||
bodyB->m_sweep.orientation = qB;
|
||||
}
|
||||
|
||||
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
|
||||
}
|
||||
69
src/bounce/softbody/softbody_mesh.cpp
Normal file
69
src/bounce/softbody/softbody_mesh.cpp
Normal file
@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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_mesh.h>
|
||||
#include <bounce/meshgen/sphere_mesh.h>
|
||||
|
||||
b3QSoftBodyMesh::b3QSoftBodyMesh()
|
||||
{
|
||||
vertexCount = 0;
|
||||
vertices = nullptr;
|
||||
tetrahedronCount = 0;
|
||||
tetrahedrons = nullptr;
|
||||
}
|
||||
|
||||
b3QSoftBodyMesh::~b3QSoftBodyMesh()
|
||||
{
|
||||
b3Free(vertices);
|
||||
b3Free(tetrahedrons);
|
||||
}
|
||||
|
||||
void b3QSoftBodyMesh::SetAsSphere(float32 radius, u32 subdivisions)
|
||||
{
|
||||
smMesh mesh;
|
||||
smCreateMesh(mesh, subdivisions);
|
||||
|
||||
vertexCount = 1 + mesh.vertexCount;
|
||||
vertices = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3));
|
||||
vertices[0].SetZero();
|
||||
for (u32 i = 0; i < mesh.vertexCount; ++i)
|
||||
{
|
||||
vertices[1 + i] = mesh.vertices[i];
|
||||
}
|
||||
|
||||
tetrahedronCount = mesh.indexCount / 3;
|
||||
tetrahedrons = (b3SoftBodyMeshTetrahedron*)b3Alloc(tetrahedronCount * sizeof(b3SoftBodyMeshTetrahedron));
|
||||
for (u32 i = 0; i < mesh.indexCount / 3; ++i)
|
||||
{
|
||||
u32 v1 = mesh.indices[3 * i + 0];
|
||||
u32 v2 = mesh.indices[3 * i + 1];
|
||||
u32 v3 = mesh.indices[3 * i + 2];
|
||||
|
||||
b3SoftBodyMeshTetrahedron* t = tetrahedrons + i;
|
||||
|
||||
t->v1 = 1 + v3;
|
||||
t->v2 = 1 + v2;
|
||||
t->v3 = 1 + v1;
|
||||
t->v4 = 0;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < vertexCount; ++i)
|
||||
{
|
||||
vertices[i] *= radius;
|
||||
}
|
||||
}
|
||||
397
src/bounce/softbody/softbody_solver.cpp
Normal file
397
src/bounce/softbody/softbody_solver.cpp
Normal file
@ -0,0 +1,397 @@
|
||||
/*
|
||||
* 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_solver.h>
|
||||
#include <bounce/softbody/softbody_mesh.h>
|
||||
#include <bounce/softbody/softbody_node.h>
|
||||
#include <bounce/softbody/softbody.h>
|
||||
#include <bounce/softbody/sparse_mat33.h>
|
||||
#include <bounce/softbody/softbody_contact_solver.h>
|
||||
|
||||
#include <bounce/dynamics/body.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
|
||||
// This work is based on the paper "Interactive Virtual Materials" written by
|
||||
// Matthias Mueller Fischer
|
||||
// The paper is available here:
|
||||
// http://matthias-mueller-fischer.ch/publications/GI2004.pdf
|
||||
|
||||
// In order to support velocity constraints on node velocities
|
||||
// we solve Ax = b using a Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
|
||||
|
||||
// Number of MPCG iterations, a value that is normally small when small time steps are taken.
|
||||
u32 b3_softBodySolverIterations = 0;
|
||||
|
||||
// Enables the stiffness warping solver.
|
||||
// bool b3_enableStiffnessWarping = true;
|
||||
|
||||
b3SoftBodySolver::b3SoftBodySolver(const b3SoftBodySolverDef& def)
|
||||
{
|
||||
m_allocator = def.stack;
|
||||
m_mesh = def.mesh;
|
||||
m_nodes = def.nodes;
|
||||
m_elements = def.elements;
|
||||
}
|
||||
|
||||
b3SoftBodySolver::~b3SoftBodySolver()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
// https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf
|
||||
static B3_FORCE_INLINE void b3ExtractRotation(b3Mat33& out, b3Quat& q, const b3Mat33& A, u32 maxIterations = 20)
|
||||
{
|
||||
for (u32 iteration = 0; iteration < maxIterations; ++iteration)
|
||||
{
|
||||
b3Mat33 R = b3QuatMat33(q);
|
||||
|
||||
float32 s = b3Abs(b3Dot(R.x, A.x) + b3Dot(R.y, A.y) + b3Dot(R.z, A.z));
|
||||
|
||||
if (s == 0.0f)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
float32 inv_s = 1.0f / s + 1.0e-9f;
|
||||
|
||||
b3Vec3 v = b3Cross(R.x, A.x) + b3Cross(R.y, A.y) + b3Cross(R.z, A.z);
|
||||
|
||||
b3Vec3 omega = inv_s * v;
|
||||
|
||||
float32 w = b3Length(omega);
|
||||
|
||||
if (w < 1.0e-9f)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
b3Quat omega_q(omega / w, w);
|
||||
|
||||
q = omega_q * q;
|
||||
q.Normalize();
|
||||
}
|
||||
|
||||
out = b3QuatMat33(q);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3SolveMPCG(b3DenseVec3& x,
|
||||
const b3SparseMat33& A, const b3DenseVec3& b,
|
||||
const b3DenseVec3& x0, const b3DiagMat33& S, u32 maxIterations = 20)
|
||||
{
|
||||
b3DiagMat33 P(A.rowCount);
|
||||
b3DiagMat33 invP(A.rowCount);
|
||||
for (u32 i = 0; i < A.rowCount; ++i)
|
||||
{
|
||||
b3Mat33 D = A(i, i);
|
||||
|
||||
// Sylvester Criterion to ensure PD-ness
|
||||
B3_ASSERT(b3Det(D.x, D.y, D.z) > 0.0f);
|
||||
|
||||
B3_ASSERT(D.x.x != 0.0f);
|
||||
float32 xx = 1.0f / D.x.x;
|
||||
|
||||
B3_ASSERT(D.y.y != 0.0f);
|
||||
float32 yy = 1.0f / D.y.y;
|
||||
|
||||
B3_ASSERT(D.z.z != 0.0f);
|
||||
float32 zz = 1.0f / D.z.z;
|
||||
|
||||
P[i] = b3Diagonal(xx, yy, zz);
|
||||
invP[i] = b3Diagonal(D.x.x, D.y.y, D.z.z);
|
||||
}
|
||||
|
||||
x = x0;
|
||||
|
||||
float32 delta_0 = b3Dot(S * b, P * (S * b));
|
||||
|
||||
b3DenseVec3 r = S * (b - A * x);
|
||||
b3DenseVec3 c = S * (invP * r);
|
||||
|
||||
float32 delta_new = b3Dot(r, c);
|
||||
|
||||
u32 iteration = 0;
|
||||
for (;;)
|
||||
{
|
||||
if (iteration == maxIterations)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (delta_new <= B3_EPSILON * B3_EPSILON * delta_0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
b3DenseVec3 q = S * (A * c);
|
||||
|
||||
float32 alpha = delta_new / b3Dot(c, q);
|
||||
|
||||
x = x + alpha * c;
|
||||
r = r - alpha * q;
|
||||
|
||||
b3DenseVec3 s = invP * r;
|
||||
|
||||
float32 delta_old = delta_new;
|
||||
|
||||
delta_new = b3Dot(r, s);
|
||||
|
||||
float32 beta = delta_new / delta_old;
|
||||
|
||||
c = S * (s + beta * c);
|
||||
|
||||
++iteration;
|
||||
}
|
||||
|
||||
b3_softBodySolverIterations = iteration;
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN)
|
||||
{
|
||||
B3_ASSERT(AN == BM);
|
||||
|
||||
for (u32 i = 0; i < AM; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < BN; ++j)
|
||||
{
|
||||
C[i + AM * j] = 0.0f;
|
||||
|
||||
for (u32 k = 0; k < AN; ++k)
|
||||
{
|
||||
C[i + AM * j] += A[i + AM * k] * B[k + BM * j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
|
||||
{
|
||||
float32 h = dt;
|
||||
|
||||
b3SparseMat33 M(m_mesh->vertexCount);
|
||||
b3DenseVec3 x(m_mesh->vertexCount);
|
||||
b3DenseVec3 p(m_mesh->vertexCount);
|
||||
b3DenseVec3 v(m_mesh->vertexCount);
|
||||
b3DenseVec3 fe(m_mesh->vertexCount);
|
||||
b3DenseVec3 x0(m_mesh->vertexCount);
|
||||
b3DiagMat33 S(m_mesh->vertexCount);
|
||||
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
M(i, i) = b3Diagonal(n->m_mass);
|
||||
x[i] = m_mesh->vertices[i];
|
||||
p[i] = n->m_position;
|
||||
v[i] = n->m_velocity;
|
||||
fe[i] = n->m_force;
|
||||
x0[i] = n->m_velocity;
|
||||
|
||||
// Apply gravity
|
||||
if (n->m_type == e_dynamicSoftBodyNode)
|
||||
{
|
||||
fe[i] += n->m_mass * gravity;
|
||||
S[i].SetIdentity();
|
||||
}
|
||||
else
|
||||
{
|
||||
S[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
// Element assembly
|
||||
b3SparseMat33 K(m_mesh->vertexCount);
|
||||
|
||||
b3DenseVec3 f0(m_mesh->vertexCount);
|
||||
f0.SetZero();
|
||||
|
||||
for (u32 ei = 0; ei < m_mesh->tetrahedronCount; ++ei)
|
||||
{
|
||||
b3SoftBodyMeshTetrahedron* mt = m_mesh->tetrahedrons + ei;
|
||||
b3SoftBodyElement* e = m_elements + ei;
|
||||
|
||||
b3Mat33* Ke = e->K;
|
||||
|
||||
u32 v1 = mt->v1;
|
||||
u32 v2 = mt->v2;
|
||||
u32 v3 = mt->v3;
|
||||
u32 v4 = mt->v4;
|
||||
|
||||
b3Vec3 p1 = p[v1];
|
||||
b3Vec3 p2 = p[v2];
|
||||
b3Vec3 p3 = p[v3];
|
||||
b3Vec3 p4 = p[v4];
|
||||
|
||||
float32 P[16] =
|
||||
{
|
||||
p1.x, p1.y, p1.z, 1.0f,
|
||||
p2.x, p2.y, p2.z, 1.0f,
|
||||
p3.x, p3.y, p3.z, 1.0f,
|
||||
p4.x, p4.y, p4.z, 1.0f
|
||||
};
|
||||
|
||||
float32 P_invP[16];
|
||||
b3Mul(P_invP, P, 4, 4, e->invP, 4, 4);
|
||||
|
||||
b3Mat33 A;
|
||||
for (u32 i = 0; i < 3; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < 3; ++j)
|
||||
{
|
||||
A(i, j) = P_invP[i + 4 * j];
|
||||
}
|
||||
}
|
||||
|
||||
b3Mat33 R;
|
||||
b3ExtractRotation(R, e->q, A);
|
||||
|
||||
b3Mat33 RT = b3Transpose(R);
|
||||
|
||||
u32 vs[4] = { v1, v2, v3, v4 };
|
||||
|
||||
for (u32 i = 0; i < 4; ++i)
|
||||
{
|
||||
u32 vi = vs[i];
|
||||
|
||||
for (u32 j = 0; j < 4; ++j)
|
||||
{
|
||||
u32 vj = vs[j];
|
||||
|
||||
K(vi, vj) += R * Ke[i + 4 * j] * RT;
|
||||
}
|
||||
}
|
||||
|
||||
b3Vec3 x1 = x[v1];
|
||||
b3Vec3 x2 = x[v2];
|
||||
b3Vec3 x3 = x[v3];
|
||||
b3Vec3 x4 = x[v4];
|
||||
|
||||
b3Vec3 xs[4] = { x1, x2, x3, x4 };
|
||||
|
||||
b3Vec3 f0s[4];
|
||||
|
||||
for (u32 i = 0; i < 4; ++i)
|
||||
{
|
||||
f0s[i].SetZero();
|
||||
|
||||
for (u32 j = 0; j < 4; ++j)
|
||||
{
|
||||
f0s[i] += R * Ke[i + 4 * j] * xs[j];
|
||||
}
|
||||
}
|
||||
|
||||
f0[v1] += f0s[0];
|
||||
f0[v2] += f0s[1];
|
||||
f0[v3] += f0s[2];
|
||||
f0[v4] += f0s[3];
|
||||
}
|
||||
|
||||
f0 = -f0;
|
||||
|
||||
b3SparseMat33 A = M + h * h * K;
|
||||
|
||||
b3DenseVec3 b = M * v - h * (K * p + f0 - fe);
|
||||
|
||||
// Solve Ax = b
|
||||
b3DenseVec3 sx(m_mesh->vertexCount);
|
||||
b3SolveMPCG(sx, A, b, x0, S);
|
||||
|
||||
// Solve constraints
|
||||
b3SoftBodyContactSolverDef contactSolverDef;
|
||||
contactSolverDef.allocator = m_allocator;
|
||||
contactSolverDef.positions = &p;
|
||||
contactSolverDef.velocities = &sx;
|
||||
contactSolverDef.bodyContactCapacity = m_mesh->vertexCount;
|
||||
|
||||
b3SoftBodyContactSolver contactSolver(contactSolverDef);
|
||||
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
if (m_nodes[i].m_bodyContact.active)
|
||||
{
|
||||
contactSolver.Add(&m_nodes[i].m_bodyContact);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
contactSolver.InitializeBodyContactConstraints();
|
||||
}
|
||||
|
||||
{
|
||||
contactSolver.WarmStart();
|
||||
}
|
||||
|
||||
// Solve velocity constraints
|
||||
{
|
||||
for (u32 i = 0; i < velocityIterations; ++i)
|
||||
{
|
||||
contactSolver.SolveBodyContactVelocityConstraints();
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
contactSolver.StoreImpulses();
|
||||
}
|
||||
|
||||
// Integrate velocities
|
||||
p = p + h * sx;
|
||||
|
||||
// Solve position constraints
|
||||
{
|
||||
bool positionSolved = false;
|
||||
for (u32 i = 0; i < positionIterations; ++i)
|
||||
{
|
||||
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
|
||||
|
||||
if (bodyContactsSolved)
|
||||
{
|
||||
positionSolved = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Synchronize bodies
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
b3NodeBodyContact* c = &n->m_bodyContact;
|
||||
|
||||
if (c->active == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Body* body = c->s2->GetBody();
|
||||
|
||||
body->SynchronizeTransform();
|
||||
|
||||
body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
|
||||
|
||||
body->SynchronizeShapes();
|
||||
}
|
||||
|
||||
// Copy state buffers back to the nodes
|
||||
for (u32 i = 0; i < m_mesh->vertexCount; ++i)
|
||||
{
|
||||
b3SoftBodyNode* n = m_nodes + i;
|
||||
|
||||
n->m_position = p[i];
|
||||
n->m_velocity = sx[i];
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user