diff --git a/examples/hello_world/main.cpp b/examples/hello_world/main.cpp index 5715060..d68db49 100644 --- a/examples/hello_world/main.cpp +++ b/examples/hello_world/main.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/framework/body_dragger.cpp b/examples/testbed/framework/body_dragger.cpp new file mode 100644 index 0000000..5e0be78 --- /dev/null +++ b/examples/testbed/framework/body_dragger.cpp @@ -0,0 +1,98 @@ +/* +* 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 + +b3BodyDragger::b3BodyDragger(b3Ray3* ray, b3World* world) +{ + m_ray = ray; + m_world = world; + m_shape = nullptr; + m_mouseJoint = nullptr; +} + +b3BodyDragger::~b3BodyDragger() +{ + +} + +bool b3BodyDragger::StartDragging() +{ + B3_ASSERT(IsDragging() == false); + + b3RayCastSingleOutput out; + if (m_world->RayCastSingle(&out, m_ray->A(), m_ray->B()) == false) + { + return false; + } + + m_x = out.fraction; + m_shape = out.shape; + + b3BodyDef bd; + b3Body* groundBody = m_world->CreateBody(bd); + + b3Body* body = m_shape->GetBody(); + body->SetAwake(true); + + b3MouseJointDef jd; + jd.bodyA = groundBody; + jd.bodyB = body; + jd.target = out.point; + jd.maxForce = 2000.0f * body->GetMass(); + + m_mouseJoint = (b3MouseJoint*)m_world->CreateJoint(jd); + + m_p = body->GetLocalPoint(out.point); + + return true; +} + +void b3BodyDragger::Drag() +{ + B3_ASSERT(IsDragging() == true); + m_mouseJoint->SetTarget(GetPointB()); +} + +void b3BodyDragger::StopDragging() +{ + B3_ASSERT(IsDragging() == true); + b3Body* groundBody = m_mouseJoint->GetBodyA(); + m_world->DestroyJoint(m_mouseJoint); + m_mouseJoint = nullptr; + m_world->DestroyBody(groundBody); + m_shape = nullptr; +} + +b3Body* b3BodyDragger::GetBody() const +{ + B3_ASSERT(IsDragging() == true); + return m_shape->GetBody(); +} + +b3Vec3 b3BodyDragger::GetPointA() const +{ + B3_ASSERT(IsDragging() == true); + return m_shape->GetBody()->GetWorldPoint(m_p); +} + +b3Vec3 b3BodyDragger::GetPointB() const +{ + B3_ASSERT(IsDragging() == true); + return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); +} \ No newline at end of file diff --git a/examples/testbed/framework/body_dragger.h b/examples/testbed/framework/body_dragger.h new file mode 100644 index 0000000..d7f4858 --- /dev/null +++ b/examples/testbed/framework/body_dragger.h @@ -0,0 +1,72 @@ +/* +* 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_BODY_DRAGGER_H +#define B3_BODY_DRAGGER_H + +#include +#include +#include +#include +#include +#include + +// A body shape dragger. +class b3BodyDragger +{ +public: + b3BodyDragger(b3Ray3* ray, b3World* world); + ~b3BodyDragger(); + + bool StartDragging(); + + void Drag(); + + void StopDragging(); + + bool IsDragging() const; + + b3Ray3* GetRay() const; + + b3Body* GetBody() const; + + b3Vec3 GetPointA() const; + + b3Vec3 GetPointB() const; +private: + b3Ray3 * m_ray; + float32 m_x; + + b3World* m_world; + + b3Shape* m_shape; + b3Vec3 m_p; + b3MouseJoint* m_mouseJoint; +}; + +inline bool b3BodyDragger::IsDragging() const +{ + return m_shape != nullptr; +} + +inline b3Ray3* b3BodyDragger::GetRay() const +{ + return m_ray; +} + +#endif \ No newline at end of file diff --git a/examples/testbed/framework/cloth_dragger.cpp b/examples/testbed/framework/cloth_dragger.cpp new file mode 100644 index 0000000..40467c9 --- /dev/null +++ b/examples/testbed/framework/cloth_dragger.cpp @@ -0,0 +1,185 @@ +/* +* 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 + +b3ClothDragger::b3ClothDragger(b3Ray3* ray, b3Cloth* cloth) +{ + m_staticDrag = true; + m_ray = ray; + m_cloth = cloth; + m_triangle = nullptr; + m_km = 10000.0f; + m_kd = 0.0f; +} + +b3ClothDragger::~b3ClothDragger() +{ + +} + +bool b3ClothDragger::StartDragging() +{ + B3_ASSERT(IsDragging() == false); + + b3ClothRayCastSingleOutput rayOut; + if (m_cloth->RayCastSingle(&rayOut, m_ray->A(), m_ray->B()) == false) + { + return false; + } + + m_mesh = m_cloth->GetMesh(); + m_triangleIndex = rayOut.triangle; + m_triangle = m_mesh->triangles + m_triangleIndex; + m_x = rayOut.fraction; + + b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1); + b3Particle* p2 = m_cloth->GetParticle(m_triangle->v2); + b3Particle* p3 = m_cloth->GetParticle(m_triangle->v3); + + b3Vec3 v1 = p1->GetPosition(); + b3Vec3 v2 = p2->GetPosition(); + b3Vec3 v3 = p3->GetPosition(); + + b3Vec3 B = GetPointB(); + + float32 wABC[4]; + b3BarycentricCoordinates(wABC, v1, v2, v3, B); + + if (wABC[3] > B3_EPSILON) + { + m_u = wABC[0] / wABC[3]; + m_v = wABC[1] / wABC[3]; + } + else + { + m_u = m_v = 0.0f; + } + + if (m_staticDrag) + { + m_t1 = p1->GetType(); + p1->SetType(e_staticParticle); + + m_t2 = p2->GetType(); + p2->SetType(e_staticParticle); + + m_t3 = p3->GetType(); + p3->SetType(e_staticParticle); + } + else + { + b3ParticleDef pd; + pd.type = e_staticParticle; + pd.position = GetPointA(); + + m_particle = m_cloth->CreateParticle(pd); + + b3ClothTriangle* triangle = m_cloth->GetTriangle(m_triangleIndex); + + b3MouseForceDef def; + def.particle = m_particle; + def.triangle = triangle; + def.w2 = m_u; + def.w3 = m_v; + def.w4 = (1.0f - m_u - m_v); + def.mouse = m_km; + def.damping = m_kd; + + m_mf = (b3MouseForce*)m_cloth->CreateForce(def); + } + + return true; +} + +void b3ClothDragger::Drag() +{ + B3_ASSERT(IsDragging() == true); + + b3Vec3 A = GetPointA(); + b3Vec3 B = GetPointB(); + + b3Vec3 dx = B - A; + + if (m_staticDrag) + { + b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1); + p1->ApplyTranslation(dx); + + b3Particle* p2 = m_cloth->GetParticle(m_triangle->v2); + p2->ApplyTranslation(dx); + + b3Particle* p3 = m_cloth->GetParticle(m_triangle->v3); + p3->ApplyTranslation(dx); + } + else + { + m_particle->SetPosition(B); + } +} + +void b3ClothDragger::SetStaticDrag(bool bit) +{ + if (bit == m_staticDrag) + { + return; + } + + if (IsDragging()) + { + StopDragging(); + } + + m_staticDrag = bit; +} + +void b3ClothDragger::StopDragging() +{ + B3_ASSERT(IsDragging() == true); + + if (m_staticDrag) + { + m_cloth->GetParticle(m_triangle->v1)->SetType(m_t1); + m_cloth->GetParticle(m_triangle->v2)->SetType(m_t2); + m_cloth->GetParticle(m_triangle->v3)->SetType(m_t3); + } + else + { + m_cloth->DestroyForce(m_mf); + m_cloth->DestroyParticle(m_particle); + } + + m_triangle = nullptr; +} + +b3Vec3 b3ClothDragger::GetPointA() const +{ + B3_ASSERT(IsDragging() == true); + + b3Vec3 A = m_cloth->GetParticle(m_triangle->v1)->GetPosition(); + b3Vec3 B = m_cloth->GetParticle(m_triangle->v2)->GetPosition(); + b3Vec3 C = m_cloth->GetParticle(m_triangle->v3)->GetPosition(); + + return m_u * A + m_v * B + (1.0f - m_u - m_v) * C; +} + +b3Vec3 b3ClothDragger::GetPointB() const +{ + B3_ASSERT(IsDragging() == true); + return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); +} \ No newline at end of file diff --git a/examples/testbed/framework/cloth_dragger.h b/examples/testbed/framework/cloth_dragger.h new file mode 100644 index 0000000..9661570 --- /dev/null +++ b/examples/testbed/framework/cloth_dragger.h @@ -0,0 +1,108 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_DRAGGER_H +#define B3_CLOTH_DRAGGER_H + +#include +#include +#include +#include +#include +#include + +// A cloth triangle dragger. +class b3ClothDragger +{ +public: + b3ClothDragger(b3Ray3* ray, b3Cloth* cloth); + ~b3ClothDragger(); + + void SetStaticDrag(bool bit); + + bool GetStaticDrag() const; + + void SetMouseStiffness(float32 k); + + float32 GetMouseStiffness(); + + void SetMouseDamping(float32 k); + + float32 GetMouseDamping(); + + bool IsDragging() const; + + bool StartDragging(); + + void Drag(); + + void StopDragging(); + + b3Vec3 GetPointA() const; + + b3Vec3 GetPointB() const; +private: + b3Ray3* m_ray; + float32 m_x; + + b3Cloth* m_cloth; + const b3ClothMesh* m_mesh; + u32 m_triangleIndex; + b3ClothMeshTriangle* m_triangle; + float32 m_u, m_v; + + float32 m_km; + float32 m_kd; + b3Particle* m_particle; + b3MouseForce* m_mf; + + bool m_staticDrag; + b3ParticleType m_t1, m_t2, m_t3; +}; + +inline bool b3ClothDragger::GetStaticDrag() const +{ + return m_staticDrag; +} + +inline void b3ClothDragger::SetMouseStiffness(float32 k) +{ + m_km = k; +} + +inline float32 b3ClothDragger::GetMouseStiffness() +{ + return m_km; +} + +inline void b3ClothDragger::SetMouseDamping(float32 k) +{ + m_kd = k; +} + +inline float32 b3ClothDragger::GetMouseDamping() +{ + return m_kd; +} + +inline bool b3ClothDragger::IsDragging() const +{ + return m_triangle != nullptr; +} + +#endif \ No newline at end of file diff --git a/examples/testbed/framework/draw.cpp b/examples/testbed/framework/draw.cpp index 480e66f..ba206e6 100644 --- a/examples/testbed/framework/draw.cpp +++ b/examples/testbed/framework/draw.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -251,24 +251,9 @@ void Draw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 co void Draw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) { - // Build a tangent vector to normal. - b3Vec3 u = b3Cross(normal, b3Vec3(1.0f, 0.0f, 0.0f)); - b3Vec3 v = b3Cross(normal, b3Vec3(0.0f, 1.0f, 0.0f)); + b3Vec3 n1, n3; + b3ComputeBasis(normal, n1, n3); - // Handle edge cases (zero cross product). - b3Vec3 n1; - if (b3LengthSquared(u) > b3LengthSquared(v)) - { - n1 = u; - } - else - { - n1 = v; - } - - n1.Normalize(); - - // Build a quaternion to rotate the tangent about the normal. u32 kEdgeCount = 20; float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); b3Quat q(normal, kAngleInc); @@ -292,24 +277,9 @@ void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 r b3Color fillColor(color.r, color.g, color.b, color.a); b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f); - // Build a tangent vector to normal. - b3Vec3 u = b3Cross(normal, b3Vec3(1.0f, 0.0f, 0.0f)); - b3Vec3 v = b3Cross(normal, b3Vec3(0.0f, 1.0f, 0.0f)); + b3Vec3 n1, n3; + b3ComputeBasis(normal, n1, n3); - // Handle edge cases (zero cross product). - b3Vec3 n1; - if (b3LengthSquared(u) > b3LengthSquared(v)) - { - n1 = u; - } - else - { - n1 = v; - } - - n1.Normalize(); - - // Build a quaternion to rotate the tangent about the normal. const u32 kEdgeCount = 20; const float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); b3Quat q(normal, kAngleInc); @@ -332,16 +302,16 @@ void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 r void Draw::DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color) { b3Transform xf; - xf.SetIdentity(); + xf.rotation.SetIdentity(); xf.position = center; m_wire->DrawSphere(radius, color, xf); } -void Draw::DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color) +void Draw::DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Mat33& rotation, const b3Color& color) { b3Transform xf; - xf.SetIdentity(); + xf.rotation = rotation; xf.position = center; m_solid->DrawSphere(radius, color, xf); @@ -352,10 +322,10 @@ void Draw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const float32 height = b3Length(c1 - c2); { - b3Transform xfc; - xfc.rotation.SetIdentity(); - xfc.position = c1; - m_wire->DrawSphere(radius, color, xfc); + b3Transform xf; + xf.rotation.SetIdentity(); + xf.position = c1; + m_wire->DrawSphere(radius, color, xf); } if (height > 0.0f) @@ -363,23 +333,23 @@ void Draw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const DrawSegment(c1, c2, color); { - b3Transform xfc; - xfc.rotation.SetIdentity(); - xfc.position = c2; - m_wire->DrawSphere(radius, color, xfc); + b3Transform xf; + xf.rotation.SetIdentity(); + xf.position = c2; + m_wire->DrawSphere(radius, color, xf); } } } -void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Color& c) +void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Mat33& rotation, const b3Color& c) { float32 height = b3Length(c1 - c2); { - b3Transform xfc; - xfc.rotation.SetIdentity(); - xfc.position = c1; - m_solid->DrawSphere(radius, c, xfc); + b3Transform xf; + xf.rotation = rotation; + xf.position = c1; + m_solid->DrawSphere(radius, c, xf); } if (height > 0.0f) @@ -387,21 +357,20 @@ void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, { b3Mat33 R; R.y = (1.0f / height) * (c1 - c2); - R.z = b3Perp(R.y); - R.x = b3Cross(R.y, R.z); + b3ComputeBasis(R.y, R.z, R.x); - b3Transform xfc; - xfc.position = 0.5f * (c1 + c2); - xfc.rotation = R; + b3Transform xf; + xf.position = 0.5f * (c1 + c2); + xf.rotation = R; - m_solid->DrawCylinder(radius, height, c, xfc); + m_solid->DrawCylinder(radius, height, c, xf); } { - b3Transform xfc; - xfc.rotation.SetIdentity(); - xfc.position = c2; - m_solid->DrawSphere(radius, c, xfc); + b3Transform xf; + xf.rotation = rotation; + xf.position = c2; + m_solid->DrawSphere(radius, c, xf); } } } @@ -410,10 +379,6 @@ void Draw::DrawTransform(const b3Transform& xf) { float32 lenght = 1.0f; - b3Color red(1.0f, 0.0f, 0.0f, 1.0f); - b3Color green(0.0f, 1.0f, 0.0f, 1.0f); - b3Color blue(0.0f, 0.0f, 1.0f, 1.0f); - b3Vec3 position = xf.position; b3Mat33 rotation = xf.rotation; @@ -421,9 +386,9 @@ void Draw::DrawTransform(const b3Transform& xf) b3Vec3 B = position + lenght * rotation.y; b3Vec3 C = position + lenght * rotation.z; - DrawSegment(position, A, red); - DrawSegment(position, B, green); - DrawSegment(position, C, blue); + DrawSegment(position, A, b3Color_red); + DrawSegment(position, B, b3Color_green); + DrawSegment(position, C, b3Color_blue); } void Draw::DrawAABB(const b3AABB3& aabb, const b3Color& color) @@ -460,6 +425,54 @@ void Draw::DrawAABB(const b3AABB3& aabb, const b3Color& color) DrawSegment(vs[1], vs[7], color); } +void Draw::DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) +{ + b3Vec3 n1, n2; + b3ComputeBasis(normal, n1, n2); + + float32 scale = 2.0f * radius; + + // v1__v4 + // | | + // v2__v3 + b3Vec3 v1 = center - scale * n1 - scale * n2; + b3Vec3 v2 = center + scale * n1 - scale * n2; + b3Vec3 v3 = center + scale * n1 + scale * n2; + b3Vec3 v4 = center - scale * n1 + scale * n2; + + DrawSegment(v1, v2, color); + DrawSegment(v2, v3, color); + DrawSegment(v3, v4, color); + DrawSegment(v4, v1, color); + + DrawSegment(center, center + normal, color); +} + +void Draw::DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) +{ + b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f); + + b3Vec3 n1, n2; + b3ComputeBasis(normal, n1, n2); + + float32 scale = 2.0f * radius; + + b3Vec3 v1 = center - scale * n1 - scale * n2; + b3Vec3 v2 = center + scale * n1 - scale * n2; + b3Vec3 v3 = center + scale * n1 + scale * n2; + b3Vec3 v4 = center - scale * n1 + scale * n2; + + DrawSegment(v1, v2, frameColor); + DrawSegment(v2, v3, frameColor); + DrawSegment(v3, v4, frameColor); + DrawSegment(v4, v1, frameColor); + + DrawSegment(center, center + normal, frameColor); + + DrawSolidTriangle(normal, v1, v2, v3, color); + DrawSolidTriangle(normal, v3, v4, v1, color); +} + void Draw::DrawString(const b3Color& color, const b3Vec2& ps, const char* text, ...) { va_list args; @@ -480,13 +493,17 @@ void Draw::DrawString(const b3Color& color, const b3Vec3& pw, const char* text, va_list args; va_start(args, text); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); ImGui::SetNextWindowBgAlpha(0.0f); ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f)); - ImGui::SetNextWindowSize(ImVec2(g_camera->m_width, g_camera->m_height)); + ImGui::SetNextWindowSize(ImVec2(0.0f, 0.0f)); ImGui::Begin("Superlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar); ImGui::SetCursorPos(ImVec2(ps.x, ps.y)); ImGui::TextColoredV(ImVec4(color.r, color.g, color.b, color.a), text, args); ImGui::End(); + ImGui::PopStyleVar(); + va_end(args); } @@ -495,181 +512,18 @@ void Draw::DrawString(const b3Color& color, const char* text, ...) va_list args; va_start(args, text); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); ImGui::SetNextWindowBgAlpha(0.0f); ImGui::SetNextWindowPos(ImVec2(0.0f, 40.0f)); - ImGui::SetNextWindowSize(ImVec2(g_camera->m_width, g_camera->m_height)); + ImGui::SetNextWindowSize(ImVec2(0.0f, 0.0f)); ImGui::Begin("Overlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar); ImGui::TextColoredV(ImVec4(color.r, color.g, color.b, color.a), text, args); ImGui::End(); - + ImGui::PopStyleVar(); + va_end(args); } -void Draw::DrawSolidSphere(const b3SphereShape* s, const b3Color& c, const b3Transform& xf) -{ - b3Transform xfc; - xfc.rotation = xf.rotation; - xfc.position = xf * s->m_center; - m_solid->DrawSphere(s->m_radius, c, xfc); -} - -void Draw::DrawSolidCapsule(const b3CapsuleShape* s, const b3Color& c, const b3Transform& xf) -{ - b3Vec3 c1 = s->m_centers[0]; - b3Vec3 c2 = s->m_centers[1]; - float32 height = b3Length(c1 - c2); - float32 radius = s->m_radius; - - { - b3Transform xfc; - xfc.rotation = xf.rotation; - xfc.position = xf * c1; - m_solid->DrawSphere(radius, c, xfc); - } - - if (height > 0.0f) - { - { - b3Mat33 R; - R.y = (1.0f / height) * (c1 - c2); - R.z = b3Perp(R.y); - R.x = b3Cross(R.y, R.z); - - b3Transform xfc; - xfc.position = xf * (0.5f * (c1 + c2)); - xfc.rotation = xf.rotation * R; - - m_solid->DrawCylinder(radius, height, c, xfc); - } - - { - b3Transform xfc; - xfc.rotation = xf.rotation; - xfc.position = xf * c2; - m_solid->DrawSphere(radius, c, xfc); - } - } -} - -void Draw::DrawSolidHull(const b3HullShape* s, const b3Color& c, const b3Transform& xf) -{ - const b3Hull* hull = s->m_hull; - - for (u32 i = 0; i < hull->faceCount; ++i) - { - const b3Face* face = hull->GetFace(i); - const b3HalfEdge* begin = hull->GetEdge(face->edge); - - b3Vec3 n = xf.rotation * hull->planes[i].normal; - - const b3HalfEdge* edge = hull->GetEdge(begin->next); - do - { - u32 i1 = begin->origin; - u32 i2 = edge->origin; - const b3HalfEdge* next = hull->GetEdge(edge->next); - u32 i3 = next->origin; - - b3Vec3 p1 = xf * hull->vertices[i1]; - b3Vec3 p2 = xf * hull->vertices[i2]; - b3Vec3 p3 = xf * hull->vertices[i3]; - - m_triangles->Vertex(p1, c, n); - m_triangles->Vertex(p2, c, n); - m_triangles->Vertex(p3, c, n); - - edge = next; - } while (hull->GetEdge(edge->next) != begin); - } -} - -void Draw::DrawSolidMesh(const b3MeshShape* s, const b3Color& c, const b3Transform& xf) -{ - const b3Mesh* mesh = s->m_mesh; - for (u32 i = 0; i < mesh->triangleCount; ++i) - { - const b3Triangle* t = mesh->triangles + i; - - b3Vec3 p1 = xf * mesh->vertices[t->v1]; - b3Vec3 p2 = xf * mesh->vertices[t->v2]; - b3Vec3 p3 = xf * mesh->vertices[t->v3]; - - b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1); - n1.Normalize(); - - m_triangles->Vertex(p1, c, n1); - m_triangles->Vertex(p2, c, n1); - m_triangles->Vertex(p3, c, n1); - - b3Vec3 n2 = -n1; - - m_triangles->Vertex(p1, c, n2); - m_triangles->Vertex(p3, c, n2); - m_triangles->Vertex(p2, c, n2); - } -} - -void Draw::DrawSolidShape(const b3Shape* s, const b3Color& c, const b3Transform& xf) -{ - switch (s->GetType()) - { - case e_sphereShape: - { - DrawSolidSphere((b3SphereShape*)s, c, xf); - break; - } - case e_capsuleShape: - { - DrawSolidCapsule((b3CapsuleShape*)s, c, xf); - break; - } - case e_hullShape: - { - DrawSolidHull((b3HullShape*)s, c, xf); - break; - } - case e_meshShape: - { - DrawSolidMesh((b3MeshShape*)s, c, xf); - break; - } - default: - { - break; - } - } -} - -void Draw::DrawSolidShapes(const b3World& world) -{ - for (b3Body* b = world.GetBodyList().m_head; b; b = b->GetNext()) - { - b3Color c; - if (b->IsAwake() == false) - { - c = b3Color(0.5f, 0.25f, 0.25f, 1.0f); - } - else if (b->GetType() == e_staticBody) - { - c = b3Color(0.5f, 0.5f, 0.5f, 1.0f); - } - else if (b->GetType() == e_dynamicBody) - { - c = b3Color(1.0f, 0.5f, 0.5f, 1.0f); - } - else - { - c = b3Color(0.5f, 0.5f, 1.0f, 1.0f); - } - - b3Transform xf = b->GetTransform(); - for (b3Shape* s = b->GetShapeList().m_head; s; s = s->GetNext()) - { - DrawSolidShape(s, c, xf); - } - } -} - void Draw::Flush() { m_triangles->Flush(); diff --git a/examples/testbed/framework/draw.h b/examples/testbed/framework/draw.h index e1e6755..32acb8f 100644 --- a/examples/testbed/framework/draw.h +++ b/examples/testbed/framework/draw.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -85,11 +85,15 @@ public: void DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color); - void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color); + void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Mat33& rotation, const b3Color& color); void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color); - void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color); + void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Mat33& rotation, const b3Color& color); + + void DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color); + + void DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color); void DrawAABB(const b3AABB3& aabb, const b3Color& color); @@ -101,18 +105,6 @@ public: void DrawString(const b3Color& color, const char* string, ...); - void DrawSolidSphere(const b3SphereShape* s, const b3Color& c, const b3Transform& xf); - - void DrawSolidCapsule(const b3CapsuleShape* s, const b3Color& c, const b3Transform& xf); - - void DrawSolidHull(const b3HullShape* s, const b3Color& c, const b3Transform& xf); - - void DrawSolidMesh(const b3MeshShape* s, const b3Color& c, const b3Transform& xf); - - void DrawSolidShape(const b3Shape* s, const b3Color& c, const b3Transform& xf); - - void DrawSolidShapes(const b3World& world); - void Flush(); private: friend struct DrawPoints; diff --git a/examples/testbed/framework/draw_gl2.h b/examples/testbed/framework/draw_gl2.h index d4ab35c..16dd8fe 100644 --- a/examples/testbed/framework/draw_gl2.h +++ b/examples/testbed/framework/draw_gl2.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -28,6 +28,9 @@ #include #include +#include +#include + #define BUFFER_OFFSET(i) ((char*)NULL + (i)) extern bool g_glDrawPoints; @@ -535,72 +538,22 @@ struct DrawTriangles struct DrawWireSphere { - enum - { - e_rings = 12, - e_sectors = 12, - e_vertexCount = e_rings * e_sectors, - e_indexCount = (e_rings - 1) * (e_sectors - 1) * 8 - }; - DrawWireSphere() { - float32 R = 1.0f / float32(e_rings - 1); - float32 S = 1.0f / float32(e_sectors - 1); + smMesh mesh; + smCreateMesh(mesh, 2); - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; - b3Color cs[e_vertexCount]; - - u32 vc = 0; - for (u32 r = 0; r < e_rings; r++) - { - for (u32 s = 0; s < e_sectors; s++) - { - float32 y = sin(-0.5f * B3_PI + B3_PI * r * R); - float32 x = cos(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - float32 z = sin(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - - vs[vc].Set(x, y, z); - cs[vc] = b3Color(1.0f, 1.0f, 1.0f, 1.0f); - ++vc; - } - } - - u32 is[e_indexCount]; - - u32 ic = 0; - for (u32 r = 0; r < e_rings - 1; r++) - { - for (u32 s = 0; s < e_sectors - 1; s++) - { - u32 i1 = r * e_sectors + s; - u32 i2 = (r + 1) * e_sectors + s; - u32 i3 = (r + 1) * e_sectors + (s + 1); - u32 i4 = r * e_sectors + (s + 1); - - is[ic++] = i1; - is[ic++] = i2; - - is[ic++] = i2; - is[ic++] = i3; - - is[ic++] = i3; - is[ic++] = i4; - - is[ic++] = i4; - is[ic++] = i1; - } - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(1, &m_vboId); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboId); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -616,6 +569,8 @@ struct DrawWireSphere GLuint m_vboId; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawWire @@ -670,6 +625,7 @@ struct DrawWire b3Mat44 m3 = g_glProjectionMatrix; b3Mat44 m = m3 * m2 * m1; + glUniform4fv(m_colorUniform, 1, &c.r); glUniformMatrix4fv(m_projectionUniform, 1, GL_FALSE, &m.x.x); glBindBuffer(GL_ARRAY_BUFFER, m_sphere.m_vboId); @@ -677,7 +633,7 @@ struct DrawWire glEnableVertexAttribArray(m_vertexAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_sphere.m_iboId); - glDrawElements(GL_LINES, m_sphere.e_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_LINES, m_sphere.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glDisableVertexAttribArray(m_vertexAttribute); @@ -697,84 +653,25 @@ struct DrawWire struct DrawSolidSphere { - enum - { - e_rings = 18, - e_sectors = 18, - e_vertexCount = e_rings * e_sectors, - e_indexCount = (e_rings - 1) * (e_sectors - 1) * 6, - e_faceCount = e_indexCount / 3 - }; - DrawSolidSphere() { - float32 R = 1.0f / float32(e_rings - 1); - float32 S = 1.0f / float32(e_sectors - 1); + smMesh mesh; + smCreateMesh(mesh, 2); - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; - - u32 vc = 0; - for (u32 r = 0; r < e_rings; r++) - { - for (u32 s = 0; s < e_sectors; s++) - { - float32 a1 = 2.0f * B3_PI * float32(s) * S; - float32 c1 = cos(a1); - float32 s1 = sin(a1); - - float32 a2 = -0.5f * B3_PI + B3_PI * float32(r) * R; - float32 s2 = sin(a2); - - float32 a3 = B3_PI * float32(r) * R; - float32 s3 = sin(a3); - - float32 x = c1 * s3; - float32 y = s2; - float32 z = s1 * s3; - - b3Vec3 v(x, y, z); - v.Normalize(); - - vs[vc] = v; - ns[vc] = v; - ++vc; - } - } - - u32 is[e_indexCount]; - - u32 ic = 0; - for (u32 r = 0; r < e_rings - 1; r++) - { - for (u32 s = 0; s < e_sectors - 1; s++) - { - u32 i1 = r * e_sectors + s; - u32 i2 = (r + 1) * e_sectors + s; - u32 i3 = (r + 1) * e_sectors + (s + 1); - u32 i4 = r * e_sectors + (s + 1); - - is[ic++] = i1; - is[ic++] = i2; - is[ic++] = i3; - - is[ic++] = i1; - is[ic++] = i3; - is[ic++] = i4; - } - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(3, m_vboIds); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[1]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), ns, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.normals, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -790,100 +687,31 @@ struct DrawSolidSphere GLuint m_vboIds[2]; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawSolidCylinder { - enum - { - e_segments = 64, - e_vertexCount = e_segments * 6, - }; - DrawSolidCylinder() { - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; + cymMesh mesh; + cymCreateMesh(mesh, 20); - u32 vc = 0; - for (u32 i = 0; i < e_segments; ++i) - { - float32 t0 = 2.0f * B3_PI * float32(i) / float32(e_segments); - float32 t1 = 2.0f * B3_PI * float32(i + 1) / float32(e_segments); - - float32 c0 = cos(t0); - float32 s0 = sin(t0); - - float32 c1 = cos(t1); - float32 s1 = sin(t1); - - b3Vec3 v1; - v1.x = s0; - v1.y = -0.5f; - v1.z = c0; - - b3Vec3 v2; - v2.x = s1; - v2.y = -0.5f; - v2.z = c1; - - b3Vec3 v3; - v3.x = s1; - v3.y = 0.5f; - v3.z = c1; - - b3Vec3 v4; - v4.x = s0; - v4.y = 0.5f; - v4.z = c0; - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - n.Normalize(); - - vs[vc] = v1; - ns[vc] = n; - ++vc; - - vs[vc] = v2; - ns[vc] = n; - ++vc; - - vs[vc] = v3; - ns[vc] = n; - ++vc; - - vs[vc] = v1; - ns[vc] = n; - ++vc; - - vs[vc] = v3; - ns[vc] = n; - ++vc; - - vs[vc] = v4; - ns[vc] = n; - ++vc; - } - - u32 is[e_vertexCount]; - - u32 ic = vc; - for (u32 i = 0; i < vc; ++i) - { - is[i] = i; - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(2, m_vboIds); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[1]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), ns, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.normals, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -899,6 +727,8 @@ struct DrawSolidCylinder GLuint m_vboIds[2]; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawSolid @@ -978,7 +808,7 @@ struct DrawSolid glEnableVertexAttribArray(m_normalAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_cylinder.m_iboId); - glDrawElements(GL_TRIANGLES, m_cylinder.e_vertexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_TRIANGLES, m_cylinder.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glDisableVertexAttribArray(m_normalAttribute); @@ -1021,7 +851,7 @@ struct DrawSolid glEnableVertexAttribArray(m_normalAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_sphere.m_iboId); - glDrawElements(GL_TRIANGLES, m_sphere.e_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_TRIANGLES, m_sphere.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glDisableVertexAttribArray(m_normalAttribute); diff --git a/examples/testbed/framework/draw_gl4.h b/examples/testbed/framework/draw_gl4.h index 1cc6261..d276500 100644 --- a/examples/testbed/framework/draw_gl4.h +++ b/examples/testbed/framework/draw_gl4.h @@ -28,6 +28,9 @@ #include #include +#include +#include + #define BUFFER_OFFSET(i) ((char*)NULL + (i)) extern bool g_glDrawPoints; @@ -551,72 +554,22 @@ struct DrawTriangles struct DrawWireSphere { - enum - { - e_rings = 12, - e_sectors = 12, - e_vertexCount = e_rings * e_sectors, - e_indexCount = (e_rings - 1) * (e_sectors - 1) * 8 - }; - DrawWireSphere() { - float32 R = 1.0f / float32(e_rings - 1); - float32 S = 1.0f / float32(e_sectors - 1); + smMesh mesh; + smCreateMesh(mesh, 2); - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; - b3Color cs[e_vertexCount]; - - u32 vc = 0; - for (u32 r = 0; r < e_rings; r++) - { - for (u32 s = 0; s < e_sectors; s++) - { - float32 y = sin(-0.5f * B3_PI + B3_PI * r * R); - float32 x = cos(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - float32 z = sin(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - - vs[vc].Set(x, y, z); - cs[vc] = b3Color(1.0f, 1.0f, 1.0f, 1.0f); - ++vc; - } - } - - u32 is[e_indexCount]; - - u32 ic = 0; - for (u32 r = 0; r < e_rings - 1; r++) - { - for (u32 s = 0; s < e_sectors - 1; s++) - { - u32 i1 = r * e_sectors + s; - u32 i2 = (r + 1) * e_sectors + s; - u32 i3 = (r + 1) * e_sectors + (s + 1); - u32 i4 = r * e_sectors + (s + 1); - - is[ic++] = i1; - is[ic++] = i2; - - is[ic++] = i2; - is[ic++] = i3; - - is[ic++] = i3; - is[ic++] = i4; - - is[ic++] = i4; - is[ic++] = i1; - } - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(1, &m_vboId); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboId); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -632,6 +585,8 @@ struct DrawWireSphere GLuint m_vboId; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawWire @@ -694,7 +649,7 @@ struct DrawWire glEnableVertexAttribArray(m_vertexAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_sphere.m_iboId); - glDrawElements(GL_LINES, m_sphere.e_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_LINES, m_sphere.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); glBindBuffer(GL_ARRAY_BUFFER, 0); @@ -712,84 +667,25 @@ struct DrawWire struct DrawSolidSphere { - enum - { - e_rings = 18, - e_sectors = 18, - e_vertexCount = e_rings * e_sectors, - e_indexCount = (e_rings - 1) * (e_sectors - 1) * 6, - e_faceCount = e_indexCount / 3 - }; - DrawSolidSphere() { - float32 R = 1.0f / float32(e_rings - 1); - float32 S = 1.0f / float32(e_sectors - 1); + smMesh mesh; + smCreateMesh(mesh, 2); - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; - - u32 vc = 0; - for (u32 r = 0; r < e_rings; r++) - { - for (u32 s = 0; s < e_sectors; s++) - { - float32 a1 = 2.0f * B3_PI * float32(s) * S; - float32 c1 = cos(a1); - float32 s1 = sin(a1); - - float32 a2 = -0.5f * B3_PI + B3_PI * float32(r) * R; - float32 s2 = sin(a2); - - float32 a3 = B3_PI * float32(r) * R; - float32 s3 = sin(a3); - - float32 x = c1 * s3; - float32 y = s2; - float32 z = s1 * s3; - - b3Vec3 v(x, y, z); - v.Normalize(); - - vs[vc] = v; - ns[vc] = v; - ++vc; - } - } - - u32 is[e_indexCount]; - - u32 ic = 0; - for (u32 r = 0; r < e_rings - 1; r++) - { - for (u32 s = 0; s < e_sectors - 1; s++) - { - u32 i1 = r * e_sectors + s; - u32 i2 = (r + 1) * e_sectors + s; - u32 i3 = (r + 1) * e_sectors + (s + 1); - u32 i4 = r * e_sectors + (s + 1); - - is[ic++] = i1; - is[ic++] = i2; - is[ic++] = i3; - - is[ic++] = i1; - is[ic++] = i3; - is[ic++] = i4; - } - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(3, m_vboIds); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[1]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), ns, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -805,100 +701,31 @@ struct DrawSolidSphere GLuint m_vboIds[2]; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawSolidCylinder { - enum - { - e_segments = 64, - e_vertexCount = e_segments * 6, - }; - DrawSolidCylinder() { - b3Vec3 vs[e_vertexCount]; - b3Vec3 ns[e_vertexCount]; + cymMesh mesh; + cymCreateMesh(mesh, 20); - u32 vc = 0; - for (u32 i = 0; i < e_segments; ++i) - { - float32 t0 = 2.0f * B3_PI * float32(i) / float32(e_segments); - float32 t1 = 2.0f * B3_PI * float32(i + 1) / float32(e_segments); - - float32 c0 = cos(t0); - float32 s0 = sin(t0); - - float32 c1 = cos(t1); - float32 s1 = sin(t1); - - b3Vec3 v1; - v1.x = s0; - v1.y = -0.5f; - v1.z = c0; - - b3Vec3 v2; - v2.x = s1; - v2.y = -0.5f; - v2.z = c1; - - b3Vec3 v3; - v3.x = s1; - v3.y = 0.5f; - v3.z = c1; - - b3Vec3 v4; - v4.x = s0; - v4.y = 0.5f; - v4.z = c0; - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - n.Normalize(); - - vs[vc] = v1; - ns[vc] = n; - ++vc; - - vs[vc] = v2; - ns[vc] = n; - ++vc; - - vs[vc] = v3; - ns[vc] = n; - ++vc; - - vs[vc] = v1; - ns[vc] = n; - ++vc; - - vs[vc] = v3; - ns[vc] = n; - ++vc; - - vs[vc] = v4; - ns[vc] = n; - ++vc; - } - - u32 is[e_vertexCount]; - - u32 ic = vc; - for (u32 i = 0; i < vc; ++i) - { - is[i] = i; - } + m_vertexCount = mesh.vertexCount; + m_indexCount = mesh.indexCount; glGenBuffers(2, m_vboIds); glGenBuffers(1, &m_iboId); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[0]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), vs, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.vertices, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[1]); - glBufferData(GL_ARRAY_BUFFER, vc * sizeof(b3Vec3), ns, GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, m_vertexCount * sizeof(b3Vec3), mesh.normals, GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_iboId); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, ic * sizeof(u32), is, GL_STATIC_DRAW); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, m_indexCount * sizeof(u32), mesh.indices, GL_STATIC_DRAW); AssertGL(); @@ -914,6 +741,8 @@ struct DrawSolidCylinder GLuint m_vboIds[2]; GLuint m_iboId; + u32 m_vertexCount; + u32 m_indexCount; }; struct DrawSolid @@ -994,7 +823,7 @@ struct DrawSolid glEnableVertexAttribArray(m_normalAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_cylinder.m_iboId); - glDrawElements(GL_TRIANGLES, m_cylinder.e_vertexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_TRIANGLES, m_cylinder.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); glBindBuffer(GL_ARRAY_BUFFER, 0); @@ -1033,7 +862,7 @@ struct DrawSolid glEnableVertexAttribArray(m_normalAttribute); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_sphere.m_iboId); - glDrawElements(GL_TRIANGLES, m_sphere.e_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); + glDrawElements(GL_TRIANGLES, m_sphere.m_indexCount, GL_UNSIGNED_INT, BUFFER_OFFSET(0)); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); glBindBuffer(GL_ARRAY_BUFFER, 0); diff --git a/examples/testbed/framework/json_profiler.cpp b/examples/testbed/framework/json_profiler.cpp index d11fa5a..bce7c98 100644 --- a/examples/testbed/framework/json_profiler.cpp +++ b/examples/testbed/framework/json_profiler.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -18,6 +18,8 @@ #include +JsonProfiler* g_jsonProfiler = nullptr; + #define STRING(x) String(x, sizeof(x) - 1) JsonProfiler::JsonProfiler() @@ -75,7 +77,7 @@ void JsonProfiler::EndEvents() m_file = nullptr; } -void JsonProfiler::BeginEvent(i32 tid, i32 pid, const char* name, float64 t) +void JsonProfiler::BeginEvent(const char* name, float64 t) { if (!m_writer) { @@ -87,8 +89,8 @@ void JsonProfiler::BeginEvent(i32 tid, i32 pid, const char* name, float64 t) float64 scale = 1000.0; m_writer->StartObject(); - m_writer->STRING("pid"); m_writer->Int(pid); - m_writer->STRING("tid"); m_writer->Int(tid); + m_writer->STRING("pid"); m_writer->Int(0); + m_writer->STRING("tid"); m_writer->Int(0); m_writer->STRING("ts"); m_writer->Int64((u64)(t * scale)); m_writer->STRING("ph"); m_writer->String(phase, 1); m_writer->STRING("cat"); m_writer->STRING("physics"); @@ -97,7 +99,7 @@ void JsonProfiler::BeginEvent(i32 tid, i32 pid, const char* name, float64 t) m_writer->EndObject(); } -void JsonProfiler::EndEvent(i32 tid, i32 pid, const char* name, float64 t) +void JsonProfiler::EndEvent(const char* name, float64 t) { if (!m_writer) { @@ -109,8 +111,8 @@ void JsonProfiler::EndEvent(i32 tid, i32 pid, const char* name, float64 t) float64 scale = 1000.0; m_writer->StartObject(); - m_writer->STRING("pid"); m_writer->Int(pid); - m_writer->STRING("tid"); m_writer->Int(tid); + m_writer->STRING("pid"); m_writer->Int(0); + m_writer->STRING("tid"); m_writer->Int(0); m_writer->STRING("ts"); m_writer->Int64((u64)(t * scale)); m_writer->STRING("ph"); m_writer->String(phase, 1); m_writer->STRING("cat"); m_writer->STRING("physics"); diff --git a/examples/testbed/framework/json_profiler.h b/examples/testbed/framework/json_profiler.h index 3ff6fb8..dc547a5 100644 --- a/examples/testbed/framework/json_profiler.h +++ b/examples/testbed/framework/json_profiler.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -26,7 +26,7 @@ using namespace rapidjson; -// The following profiler listener is notified by a profiler when events are initiated +// The following profiler is notified when events are initiated // or terminated. // When it receives the notification it immediately saves its data into a .json file format. // The .json file can be read and interpreted by the Google Chrome Tracing. @@ -43,13 +43,15 @@ public: void EndEvents(); - void BeginEvent(i32 tid, i32 pid, const char* name, float64 time); + void BeginEvent(const char* name, float64 time); - void EndEvent(i32 tid, i32 pid, const char* name, float64 time); + void EndEvent(const char* name, float64 time); private: - FILE * m_file; + FILE* m_file; FileWriteStream* m_stream; Writer* m_writer; }; +extern JsonProfiler* g_jsonProfiler; + #endif \ No newline at end of file diff --git a/examples/testbed/framework/main.cpp b/examples/testbed/framework/main.cpp index 211d510..823170d 100644 --- a/examples/testbed/framework/main.cpp +++ b/examples/testbed/framework/main.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -31,7 +31,7 @@ #include // -static GLFWwindow* g_window; +GLFWwindow* g_window; // static Model* g_model; @@ -100,13 +100,17 @@ static void Run() int w, h; glfwGetWindowSize(g_window, &w, &h); g_view->Event_SetWindowSize(u32(w), u32(h)); - + while (glfwWindowShouldClose(g_window) == 0) { g_profiler->Begin(); - - g_profiler->PushEvent("Frame"); + g_profilerSt->Begin(); + + g_profiler->BeginScope("Frame"); + + g_profilerSt->BeginScope("Frame"); + g_view->BeginInterface(); if (g_model->IsPaused()) @@ -118,25 +122,33 @@ static void Run() g_draw->DrawString(b3Color_white, "*PLAYING*"); } - if (g_settings->drawProfile) - { - const b3Array& records = g_profilerRecorder->GetRecords(); - for (u32 i = 0; i < records.Count(); ++i) - { - const ProfilerRecord& r = records[i]; - g_draw->DrawString(b3Color_white, "%s %.4f (%.4f) [ms]", r.name, r.elapsed, r.maxElapsed); - } - } - g_view->Interface(); g_model->Update(); - g_view->EndInterface(); - - g_profiler->PopEvent(); + g_profilerSt->EndScope(); - g_profiler->End(g_profilerListener); + g_profiler->EndScope(); + + if (g_settings->drawProfileTree) + { + g_view->InterfaceProfileTree(); + } + + if (g_settings->drawProfileTreeStats) + { + g_view->InterfaceProfileTreeStats(); + } + + g_profilerSt->End(); + +#if PROFILE_JSON == 1 + g_model->UpdateJson(); +#endif + + g_profiler->End(); + + g_view->EndInterface(); glfwSwapBuffers(g_window); glfwPollEvents(); diff --git a/examples/testbed/framework/model.cpp b/examples/testbed/framework/model.cpp index 581616a..ce522d8 100644 --- a/examples/testbed/framework/model.cpp +++ b/examples/testbed/framework/model.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -26,8 +26,11 @@ Model::Model() g_draw = &m_draw; g_camera = &m_camera; g_profiler = &m_profiler; - g_profilerRecorder = &m_profilerListener.m_recorderProfiler; - g_profilerListener = &m_profilerListener; + g_profilerSt = &m_profilerSt; + +#if (PROFILE_JSON == 1) + g_jsonProfiler = &m_jsonProfiler; +#endif m_test = nullptr; @@ -53,8 +56,11 @@ Model::~Model() g_draw = nullptr; g_camera = nullptr; g_profiler = nullptr; - g_profilerRecorder = nullptr; - g_profilerListener = nullptr; + g_profilerSt = nullptr; + +#if (PROFILE_JSON == 1) + g_jsonProfiler = nullptr; +#endif delete m_test; } @@ -223,4 +229,34 @@ void Model::Update() m_test->Step(); m_draw.Flush(); -} \ No newline at end of file +} + +#if (PROFILE_JSON == 1) + +static inline void RecurseEvents(ProfilerNode* node) +{ + g_jsonProfiler->BeginEvent(node->name, node->t0); + + g_jsonProfiler->EndEvent(node->name, node->t1); + + for (u32 i = 0; i < node->children.Count(); ++i) + { + RecurseEvents(node->children[i]); + } +} + +void Model::UpdateJson() +{ + m_jsonProfiler.BeginEvents(); + + ProfilerNode* root = m_profiler.GetRoot(); + + if (root) + { + RecurseEvents(root); + } + + m_jsonProfiler.EndEvents(); +} + +#endif \ No newline at end of file diff --git a/examples/testbed/framework/model.h b/examples/testbed/framework/model.h index e41d97b..f586afa 100644 --- a/examples/testbed/framework/model.h +++ b/examples/testbed/framework/model.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -20,7 +20,15 @@ #define MODEL_H #include -#include +#include +#include + +// Set to 1 to write profile events into a .json file. Set to 0 otherwise. +#define PROFILE_JSON 0 + +#if (PROFILE_JSON == 1) +#include +#endif class Test; @@ -53,7 +61,11 @@ public: void Command_ZoomCamera(float32 d); void Update(); - + +#if (PROFILE_JSON == 1) + void UpdateJson(); +#endif + bool IsPaused() const { return m_pause; } private: friend class ViewModel; @@ -63,7 +75,12 @@ private: Draw m_draw; Camera m_camera; Profiler m_profiler; - TestbedListener m_profilerListener; + ProfilerSt m_profilerSt; + +#if (PROFILE_JSON == 1) + JsonProfiler m_jsonProfiler; +#endif + Test* m_test; bool m_setTest; bool m_pause; diff --git a/examples/testbed/framework/profiler.cpp b/examples/testbed/framework/profiler.cpp index bc72572..fea9077 100644 --- a/examples/testbed/framework/profiler.cpp +++ b/examples/testbed/framework/profiler.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,73 +19,92 @@ #include Profiler* g_profiler = nullptr; -ProfilerListener* g_profilerListener = nullptr; -Profiler::Profiler() +Profiler::Profiler() : m_pool(sizeof(ProfilerNode)) { + m_root = nullptr; m_top = nullptr; } Profiler::~Profiler() { + assert(m_root == nullptr); + assert(m_top == nullptr); } -bool Profiler::PushEvent(const char* name) +ProfilerNode* Profiler::CreateNode() +{ + void* block = m_pool.Allocate(); + ProfilerNode* n = new (block) ProfilerNode(); + return n; +} + +void Profiler::DestroyNode(ProfilerNode* node) +{ + node->~ProfilerNode(); + m_pool.Free(node); +} + +void Profiler::BeginScope(const char* name) { m_time.Update(); - ProfilerEvent e; - e.tid = -1; - e.pid = -1; - e.t0 = m_time.GetCurrentMilis(); - e.t1 = 0.0; - e.name = name; - e.parent = m_top; + ProfilerNode* n = CreateNode(); + n->name = name; + n->t0 = m_time.GetCurrentMilis(); + n->t1 = 0.0; + n->parent = m_top; - ProfilerEvent* back = m_events.Push(e); - if (back) + if (m_root == nullptr) { - m_top = back; + m_root = n; + m_top = n; + return; } - return back != NULL; + if (m_top) + { + m_top->children.PushBack(n); + } + + m_top = n; } -void Profiler::PopEvent() +void Profiler::EndScope() { - B3_ASSERT(m_top); - B3_ASSERT(m_top->t1 == 0.0); - m_time.Update(); + + assert(m_top != nullptr); m_top->t1 = m_time.GetCurrentMilis(); - B3_ASSERT(m_top->t1 != 0.0); + assert(m_top->t1 > m_top->t0); + m_top = m_top->parent; } void Profiler::Begin() { // If this assert is hit then it means Profiler::End hasn't been called. - B3_ASSERT(m_events.IsEmpty()); + assert(m_root == nullptr); + assert(m_top == nullptr); } -void Profiler::End(ProfilerListener* listener) +void Profiler::RecurseDestroyNode(ProfilerNode* node) { - listener->BeginEvents(); - - while (m_events.IsEmpty() == false) + for (u32 i = 0; i < node->children.Count(); ++i) { - const ProfilerEvent& e = m_events.Front(); - - m_events.Pop(); - - listener->BeginEvent(e.tid, e.pid, e.name, e.t0); - - listener->EndEvent(e.tid, e.pid, e.name, e.t1); - - listener->Duration(e.name, e.t1 - e.t0); + RecurseDestroyNode(node->children[i]); } - B3_ASSERT(m_events.IsEmpty()); + DestroyNode(node); +} - listener->EndEvents(); +void Profiler::End() +{ + assert(m_top == nullptr); + + if (m_root) + { + RecurseDestroyNode(m_root); + m_root = nullptr; + } } \ No newline at end of file diff --git a/examples/testbed/framework/profiler.h b/examples/testbed/framework/profiler.h index f5c2d8b..070ae7e 100644 --- a/examples/testbed/framework/profiler.h +++ b/examples/testbed/framework/profiler.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -20,28 +20,21 @@ #define PROFILER_H #include -#include -#include +#include #include +#include -// This defines the maximum number of profiler events that can be -// queued per frame until the function Profiler::Flush is called. -#define MAX_PROFILER_EVENTS 256 - -class ProfilerListener; - -// A time-stamped profiler event. -struct ProfilerEvent +// Profiler node +struct ProfilerNode { - i32 tid; - i32 pid; const char* name; float64 t0; float64 t1; - ProfilerEvent* parent; + ProfilerNode* parent; + b3StackArray children; }; -// A single-threaded event-based profiler. +// A single-threaded profiler. class Profiler { public: @@ -53,67 +46,28 @@ public: void Begin(); // Must be called after profiling. - // The function will report all events in this profiler - // to the given event listener in the correct calling order. - // This function also flushes the profiler. - void End(ProfilerListener* listener); + void End(); - // Add a profiler event to the queue. - // Return true if the even has been added to the event queue - // or false if the queue is full. - // You can control the maximum number of profiler events using - // MAX_PROFILER_EVENTS. - bool PushEvent(const char* name); + // Begin a new scope. + void BeginScope(const char* name); - // Remove the top profiler event. - void PopEvent(); + // End the top scope. + void EndScope(); + + // Get the root profiler node. + ProfilerNode* GetRoot() { return m_root; } private: - b3Time m_time; - b3BoundedQueue m_events; - ProfilerEvent* m_top; + ProfilerNode* CreateNode(); + void DestroyNode(ProfilerNode* node); + + void RecurseDestroyNode(ProfilerNode* node); + + b3BlockPool m_pool; // pool of nodes + b3Time m_time; // timer + ProfilerNode* m_root; // tree root node + ProfilerNode* m_top; // top node }; extern Profiler* g_profiler; -// Any implementation of this interface passed to Profiler::End will listen to profile events. -class ProfilerListener -{ -public: - virtual ~ProfilerListener() { } - - // This function is called when profiling has began. - virtual void BeginEvents() { } - - // This function is called when profiling has ended. - virtual void EndEvents() { } - - // This function is called when a profiler event begins. - virtual void BeginEvent(i32 tid, i32 pid, const char* name, float64 time) - { - B3_NOT_USED(tid); - B3_NOT_USED(pid); - B3_NOT_USED(name); - B3_NOT_USED(time); - } - - // This function is called when a profiler event ends. - virtual void EndEvent(i32 tid, i32 pid, const char* name, float64 time) - { - B3_NOT_USED(tid); - B3_NOT_USED(pid); - B3_NOT_USED(name); - B3_NOT_USED(time); - } - - // This function is called when a profiler event ends. - // However it supplies the duration of the last begin and end events. - virtual void Duration(const char* name, float64 duration) - { - B3_NOT_USED(name); - B3_NOT_USED(duration); - } -}; - -extern ProfilerListener* g_profilerListener; - #endif \ No newline at end of file diff --git a/examples/testbed/framework/profiler_st.cpp b/examples/testbed/framework/profiler_st.cpp new file mode 100644 index 0000000..3866e84 --- /dev/null +++ b/examples/testbed/framework/profiler_st.cpp @@ -0,0 +1,197 @@ +/* +* 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 + +ProfilerSt* g_profilerSt = nullptr; + +ProfilerSt::ProfilerSt() : m_pool(sizeof(ProfilerStNode)) +{ + m_root = nullptr; + m_top = nullptr; +} + +ProfilerSt::~ProfilerSt() +{ + assert(m_root == nullptr); + assert(m_top == nullptr); +} + +ProfilerStNodeStat* ProfilerSt::FindStat(const char* name) +{ + for (u32 i = 0; i < m_stats.Count(); ++i) + { + if (m_stats[i].name == name) + { + return &m_stats[i]; + } + } + + return nullptr; +} + +ProfilerStNodeStat* ProfilerSt::CreateStat() +{ + m_stats.PushBack(ProfilerStNodeStat()); + return &m_stats.Back(); +} + +ProfilerStNode* ProfilerSt::CreateNode() +{ + void* block = m_pool.Allocate(); + return new (block) ProfilerStNode(); +} + +void ProfilerSt::DestroyNode(ProfilerStNode* node) +{ + node->~ProfilerStNode(); + m_pool.Free(node); +} + +void ProfilerSt::RecurseDestroyNode(ProfilerStNode* node) +{ + for (u32 i = 0; i < node->children.Count(); ++i) + { + return RecurseDestroyNode(node->children[i]); + } + + DestroyNode(node); +} + +static ProfilerStNode* RecurseFindNode(ProfilerStNode* node, const char* name) +{ + if (node->name == name) + { + return node; + } + + ProfilerStNode* result = nullptr; + for (u32 i = 0; result == nullptr && i < node->children.Count(); ++i) + { + result = RecurseFindNode(node->children[i], name); + } + + return result; +} + +ProfilerStNode* ProfilerSt::FindNode(const char* name) +{ + if (m_top) + { + return RecurseFindNode(m_top, name); + } + + if (m_root) + { + return RecurseFindNode(m_root, name); + } + + return nullptr; +} + +void ProfilerSt::BeginScope(const char* name) +{ + ProfilerStNode* fn = FindNode(name); + + if (fn) + { + m_time.Update(); + fn->t0 = m_time.GetCurrentMilis(); + ++fn->callCount; + m_top = fn; + return; + } + + m_time.Update(); + + ProfilerStNode* n = CreateNode(); + n->name = name; + n->t0 = m_time.GetCurrentMilis(); + n->elapsed = 0.0f; + n->callCount = 1; + n->parent = m_top; + n->stat = nullptr; + + if (m_root == nullptr) + { + m_root = n; + m_top = n; + + return; + } + + if (m_top) + { + m_top->children.PushBack(n); + } + + m_top = n; +} + +void ProfilerSt::EndScope() +{ + assert(m_top != nullptr); + + m_time.Update(); + + m_top->t1 = m_time.GetCurrentMilis(); + + float64 elapsedTime = m_top->t1 - m_top->t0; + + m_top->elapsed += elapsedTime; + + ProfilerStNodeStat* stat = FindStat(m_top->name); + + if (stat == nullptr) + { + stat = CreateStat(); + stat->name = m_top->name; + stat->minElapsed = elapsedTime; + stat->maxElapsed = elapsedTime; + } + else + { + stat->minElapsed = b3Min(stat->minElapsed, elapsedTime); + stat->maxElapsed = b3Max(stat->maxElapsed, elapsedTime); + } + + if (m_top->stat == nullptr) + { + m_top->stat = stat; + } + + assert(m_top->stat == stat); + + m_top = m_top->parent; +} + +void ProfilerSt::Begin() +{ + assert(m_top == nullptr); +} + +void ProfilerSt::End() +{ + assert(m_top == nullptr); + + if (m_root) + { + RecurseDestroyNode(m_root); + m_root = nullptr; + } +} \ No newline at end of file diff --git a/examples/testbed/framework/profiler_st.h b/examples/testbed/framework/profiler_st.h new file mode 100644 index 0000000..9cb1290 --- /dev/null +++ b/examples/testbed/framework/profiler_st.h @@ -0,0 +1,94 @@ +/* +* 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 PROFILER_ST_H +#define PROFILER_ST_H + +#include +#include +#include +#include + +// Profiler tree node statistics +struct ProfilerStNodeStat +{ + const char* name; + float64 minElapsed; + float64 maxElapsed; +}; + +// A profiler tree node +struct ProfilerStNode +{ + const char* name; + float64 t0; + float64 t1; + + float64 elapsed; + u32 callCount; + + ProfilerStNode* parent; + b3StackArray children; + + ProfilerStNodeStat* stat; +}; + +// A profiler tree +class ProfilerSt +{ +public: + ProfilerSt(); + + ~ProfilerSt(); + + // Must be called before profiling. + void Begin(); + + // Must be called after profiling. + void End(); + + // Begin a new scope. + void BeginScope(const char* name); + + // End the top scope. + void EndScope(); + + ProfilerStNode* GetRoot() { return m_root; } +private: + ProfilerStNode* CreateNode(); + void DestroyNode(ProfilerStNode* node); + + void RecurseDestroyNode(ProfilerStNode* node); + + ProfilerStNode* FindNode(const char* name); + + ProfilerStNodeStat* CreateStat(); + + ProfilerStNodeStat* FindStat(const char* name); + + b3BlockPool m_pool; // pool of nodes + b3Time m_time; // timer + ProfilerStNode* m_root; // tree root node + ProfilerStNode* m_top; // top node + + b3StackArray m_stats; // node statistics +}; + +extern ProfilerSt* g_profilerSt; + +#endif \ No newline at end of file diff --git a/examples/testbed/framework/softbody_dragger.cpp b/examples/testbed/framework/softbody_dragger.cpp new file mode 100644 index 0000000..1667c2e --- /dev/null +++ b/examples/testbed/framework/softbody_dragger.cpp @@ -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 + +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(); +} \ No newline at end of file diff --git a/examples/testbed/framework/softbody_dragger.h b/examples/testbed/framework/softbody_dragger.h new file mode 100644 index 0000000..e8a6a92 --- /dev/null +++ b/examples/testbed/framework/softbody_dragger.h @@ -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 +#include +#include +#include + +// 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 \ No newline at end of file diff --git a/examples/testbed/framework/test.cpp b/examples/testbed/framework/test.cpp index 39170c1..fe3ff77 100644 --- a/examples/testbed/framework/test.cpp +++ b/examples/testbed/framework/test.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -18,32 +18,27 @@ #include #include -#include +#include extern u32 b3_allocCalls, b3_maxAllocCalls; extern u32 b3_convexCalls, b3_convexCacheHits; extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters; extern bool b3_convexCache; -static bool push_ok = false; - void b3BeginProfileScope(const char* name) { - push_ok = g_profiler->PushEvent(name); + g_profiler->BeginScope(name); + g_profilerSt->BeginScope(name); } void b3EndProfileScope() { - if (push_ok) - { - g_profiler->PopEvent(); - push_ok = false; - } + g_profiler->EndScope(); + g_profilerSt->EndScope(); } Test::Test() : - m_bodyDragger(&m_ray, &m_world), - m_clothDragger(&m_ray, &m_world) + m_bodyDragger(&m_ray, &m_world) { b3Draw_draw = g_draw; b3_convexCache = g_testSettings->convexCache; @@ -93,7 +88,7 @@ void Test::Step() if (g_settings->drawTriangles) { - g_draw->DrawSolidShapes(m_world); + m_world.DrawSolid(); } if (g_settings->drawStats) @@ -121,11 +116,6 @@ void Test::Step() g_draw->DrawString(b3Color_white, "Convex Cache Hits %d (%f)", b3_convexCacheHits, convexCacheHitRatio); g_draw->DrawString(b3Color_white, "Frame Allocations %d (%d)", b3_allocCalls, b3_maxAllocCalls); } - - if (m_clothDragger.IsDragging() == true) - { - g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white); - } } void Test::MouseMove(const b3Ray3& pw) @@ -136,11 +126,6 @@ void Test::MouseMove(const b3Ray3& pw) { m_bodyDragger.Drag(); } - - if (m_clothDragger.IsDragging() == true) - { - m_clothDragger.Drag(); - } } void Test::MouseLeftDown(const b3Ray3& pw) @@ -152,14 +137,6 @@ void Test::MouseLeftDown(const b3Ray3& pw) BeginDragging(); } } - - if (m_clothDragger.IsDragging() == false) - { - if (m_clothDragger.StartDragging() == true) - { - BeginDragging(); - } - } } void Test::MouseLeftUp(const b3Ray3& pw) @@ -170,11 +147,4 @@ void Test::MouseLeftUp(const b3Ray3& pw) EndDragging(); } - - if (m_clothDragger.IsDragging() == true) - { - m_clothDragger.StopDragging(); - - EndDragging(); - } } \ No newline at end of file diff --git a/examples/testbed/framework/test.h b/examples/testbed/framework/test.h index 481c2ea..8e21ca7 100644 --- a/examples/testbed/framework/test.h +++ b/examples/testbed/framework/test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,6 +22,9 @@ #include #include +#include +#include + #include #include @@ -33,27 +36,6 @@ inline float32 RandomFloat(float32 a, float32 b) return a + r; } -class RayCastListener : public b3RayCastListener -{ -public: - float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction) - { - hit.shape = shape; - hit.point = point; - hit.normal = normal; - hit.fraction = fraction; - return 1.0f; - } - - float32 ReportCloth(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction) - { - B3_ASSERT(false); - return 1.0f; - } - - b3RayCastSingleShapeOutput hit; -}; - class Test : public b3ContactListener { public: @@ -77,10 +59,9 @@ public: void EndContact(b3Contact* c) override { } void PreSolve(b3Contact* c) override { } - b3World m_world; - b3Ray3 m_ray; - b3ClothDragger m_clothDragger; + + b3World m_world; b3BodyDragger m_bodyDragger; b3BoxHull m_groundHull; diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index d7d5475..9224561 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -53,28 +54,27 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include -#include #include #include -#include -#include +#include #include -#include +#include +#include +#include TestEntry g_tests[] = { { "Convex Hull", &ConvexHull::Create }, { "Cluster", &Cluster::Create }, { "Distance", &Distance::Create }, + { "Shape Cast", &ShapeCast::Create }, { "Capsule Collision", &CapsuleCollision::Create }, { "Hull Collision", &HullCollision::Create }, { "Deep Capsule", &DeepCapsule::Create }, @@ -106,7 +106,6 @@ TestEntry g_tests[] = { "Box Pyramid Rows", &Pyramids::Create }, { "Ray Cast", &RayCast::Create }, { "Sensor Test", &SensorTest::Create }, - { "Point & Click", &PointClick::Create }, { "Body Types", &BodyTypes::Create }, { "Varying Friction", &VaryingFriction::Create }, { "Varying Restitution", &VaryingRestitution::Create }, @@ -115,12 +114,12 @@ TestEntry g_tests[] = { "Multiple Pendulum", &MultiplePendulum::Create }, { "Table Cloth", &TableCloth::Create }, { "Pinned Cloth", &PinnedCloth::Create }, - { "Shirt", &Shirt::Create }, { "Particle Types", &ParticleTypes::Create }, { "Tension Mapping", &TensionMapping::Create }, - { "Self-Collision", &SelfCollision::Create }, - { "Mass-Spring System", &MassSpring::Create }, - { "Single Pendulum", &SinglePendulum::Create }, + { "Cloth Self-Collision", &ClothSelfCollision::Create }, + { "Beam", &Beam::Create }, + { "Pinned Soft Body", &PinnedSoftBody::Create }, + { "Smash Soft Body", &SmashSoftBody::Create }, { "Rope", &Rope::Create }, { NULL, NULL } }; diff --git a/examples/testbed/framework/testbed_listener.h b/examples/testbed/framework/testbed_listener.h deleted file mode 100644 index 35dc5d3..0000000 --- a/examples/testbed/framework/testbed_listener.h +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 TESTBED_LISTENER_H -#define TESTBED_LISTENER_H - -#include -#include - -// Set to 1 then the testbed listener will write profile events into a .json file. -// Set to 0 otherwise. -#define PROFILE_JSON 0 - -#if (PROFILE_JSON == 1) - #include -#endif - - -class TestbedListener : public ProfilerListener -{ -public: - void BeginEvents() override - { - m_recorderProfiler.BeginEvents(); - -#if (PROFILE_JSON == 1) - m_jsonListener.BeginEvents(); -#endif - - } - - void EndEvents() override - { - m_recorderProfiler.EndEvents(); - -#if (PROFILE_JSON == 1) - m_jsonListener.EndEvents(); -#endif - - } - - void BeginEvent(i32 tid, i32 pid, const char* name, float64 time) override - { -#if (PROFILE_JSON == 1) - m_jsonListener.BeginEvent(tid, pid, name, time); -#endif - - } - - void EndEvent(i32 tid, i32 pid, const char* name, float64 time) override - { -#if (PROFILE_JSON == 1) - m_jsonListener.EndEvent(tid, pid, name, time); -#endif - - } - - void Duration(const char* name, float64 time) override - { - m_recorderProfiler.Add(name, time); - } - - RecorderProfiler m_recorderProfiler; - -#if (PROFILE_JSON == 1) - JsonProfiler m_jsonListener; -#endif - -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/framework/view.cpp b/examples/testbed/framework/view.cpp index 23b03a1..0f4cffb 100644 --- a/examples/testbed/framework/view.cpp +++ b/examples/testbed/framework/view.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #if defined (U_OPENGL_2) @@ -217,7 +219,8 @@ void View::Interface() if (ImGui::BeginMenu("View")) { - ImGui::MenuItem("Profile", "", &settings.drawProfile); + ImGui::MenuItem("Profile Tree", "", &settings.drawProfileTree); + ImGui::MenuItem("Profile Tree Statistics", "", &settings.drawProfileTreeStats); ImGui::MenuItem("Statistics", "", &settings.drawStats); ImGui::Separator(); @@ -400,6 +403,111 @@ void View::Interface() ImGui::End(); } +static void TreeNode(ProfilerNode* node, u32& index) +{ + ImGui::PushID(index); + ++index; + + if (ImGui::TreeNode(node->name)) + { + float64 elapsedTime = node->t1 - node->t0; + ImGui::Text("%.4f [ms]", elapsedTime); + + for (u32 i = 0; i < node->children.Count(); ++i) + { + TreeNode(node->children[i], index); + } + ImGui::TreePop(); + } + + ImGui::PopID(); +} + +void View::InterfaceProfileTree() +{ + ImGui::Begin("Overlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar); + ImVec2 ws = ImGui::GetWindowSize(); + ImVec2 wp = ImGui::GetWindowPos(); + ImGui::End(); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + + ImGui::SetNextWindowBgAlpha(0.0f); + ImGui::SetNextWindowPos(ImVec2(0.0f, wp.y + ws.y)); + ImGui::SetNextWindowSize(ImVec2(g_camera->m_width - 250.0f, 0.0f)); + + ImGui::Begin("Profile Tree", NULL, ImGuiWindowFlags_AlwaysAutoResize); + + ProfilerNode* root = g_profiler->GetRoot(); + if (root) + { + u32 index = 0; + TreeNode(root, index); + } + + ImGui::End(); + + ImGui::PopStyleVar(); +} + +static void TreeNode(ProfilerStNode* node, u32& index) +{ + ImGui::PushID(index); + ++index; + + if (ImGui::TreeNode(node->name)) + { + ImGui::Text("%.4f (min = %.4f) (max = %.4f) (calls = %d) [ms]", node->elapsed, node->stat->minElapsed, node->stat->maxElapsed, node->callCount); + + for (u32 i = 0; i < node->children.Count(); ++i) + { + TreeNode(node->children[i], index); + } + ImGui::TreePop(); + } + + ImGui::PopID(); +} + +void View::InterfaceProfileTreeStats() +{ + ImGui::Begin("Overlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar); + ImVec2 wp = ImGui::GetWindowPos(); + ImVec2 ws = ImGui::GetWindowSize(); + ImGui::End(); + + wp.y = wp.y + ws.y; + + if (g_settings->drawProfileTree) + { + ImGui::Begin("Profile Tree", NULL, ImGuiWindowFlags_AlwaysAutoResize); + ImVec2 ptwp = ImGui::GetWindowPos(); + ImVec2 ptws = ImGui::GetWindowSize(); + ImGui::End(); + + wp.y = ptwp.y + ptws.y; + } + + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + + ImGui::SetNextWindowBgAlpha(0.0f); + ImGui::SetNextWindowPos(ImVec2(0.0f, wp.y)); + ImGui::SetNextWindowSize(ImVec2(g_camera->m_width - 250.0f, 0.0f)); + + ImGui::Begin("Profile Tree Statistics", NULL, ImGuiWindowFlags_AlwaysAutoResize); + + ProfilerStNode* root = g_profilerSt->GetRoot(); + if (root) + { + u32 index = 0; + TreeNode(root, index); + } + + ImGui::End(); + + ImGui::PopStyleVar(); +} + void View::EndInterface() { ImGui::PopStyleVar(); diff --git a/examples/testbed/framework/view.h b/examples/testbed/framework/view.h index 26c4e57..3851e0c 100644 --- a/examples/testbed/framework/view.h +++ b/examples/testbed/framework/view.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -41,6 +41,8 @@ public: void BeginInterface(); void Interface(); + void InterfaceProfileTree(); + void InterfaceProfileTreeStats(); void EndInterface(); private: friend class ViewModel; diff --git a/examples/testbed/framework/view_model.cpp b/examples/testbed/framework/view_model.cpp index 370a231..ffb3372 100644 --- a/examples/testbed/framework/view_model.cpp +++ b/examples/testbed/framework/view_model.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/framework/view_model.h b/examples/testbed/framework/view_model.h index 7d5925f..09f46ab 100644 --- a/examples/testbed/framework/view_model.h +++ b/examples/testbed/framework/view_model.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -32,7 +32,8 @@ struct Settings drawLines = true; drawTriangles = true; drawGrid = true; - drawProfile = false; + drawProfileTree = false; + drawProfileTreeStats = false; drawStats = false; } @@ -42,7 +43,8 @@ struct Settings bool drawLines; bool drawTriangles; bool drawGrid; - bool drawProfile; + bool drawProfileTree; + bool drawProfileTreeStats; bool drawStats; }; diff --git a/examples/testbed/tests/angular_motion.h b/examples/testbed/tests/angular_motion.h index a5674cc..ae3578e 100644 --- a/examples/testbed/tests/angular_motion.h +++ b/examples/testbed/tests/angular_motion.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/beam.h b/examples/testbed/tests/beam.h new file mode 100644 index 0000000..6a0a913 --- /dev/null +++ b/examples/testbed/tests/beam.h @@ -0,0 +1,162 @@ +/* +* 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 BEAM_H +#define BEAM_H + +#include + +class Beam : public Test +{ +public: + Beam() + { + // Create soft body + b3SoftBodyDef def; + def.mesh = &m_mesh; + def.density = 0.2f; + def.E = 1000.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 body + { + b3BodyDef bd; + bd.type = e_staticBody; + bd.position.x = -3.5f; + + b3Body* b = m_world.CreateBody(bd); + + m_wallHull.Set(1.0f, 5.0f, 5.0f); + + b3HullShape wallShape; + wallShape.m_hull = &m_wallHull; + + b3ShapeDef sd; + sd.shape = &wallShape; + + b3Shape* wall = b->CreateShape(sd); + } + + b3AABB3 aabb; + aabb.m_lower.Set(-3.0f, -5.0f, -5.0f); + aabb.m_upper.Set(-2.0f, 5.0f, 5.0f); + + for (u32 i = 0; i < m_mesh.vertexCount; ++i) + { + b3SoftBodyNode* n = m_body->GetVertexNode(i); + b3Vec3 p = n->GetPosition(); + if (aabb.Contains(p)) + { + n->SetType(e_staticSoftBodyNode); + } + } + + m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body); + } + + ~Beam() + { + 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 Beam(); + } + + b3BlockSoftBodyMesh<5, 2, 2> m_mesh; + + b3SoftBody* m_body; + b3SoftBodyDragger* m_bodyDragger; + + b3BoxHull m_wallHull; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/body_types.h b/examples/testbed/tests/body_types.h index ab3449a..ac71468 100644 --- a/examples/testbed/tests/body_types.h +++ b/examples/testbed/tests/body_types.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/box_edge_contact.h b/examples/testbed/tests/box_edge_contact.h index 9847880..5e90f7f 100644 --- a/examples/testbed/tests/box_edge_contact.h +++ b/examples/testbed/tests/box_edge_contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/box_face_contact.h b/examples/testbed/tests/box_face_contact.h index 99d6b9f..71243e3 100644 --- a/examples/testbed/tests/box_face_contact.h +++ b/examples/testbed/tests/box_face_contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/box_stack.h b/examples/testbed/tests/box_stack.h index 6cf636a..1d1e9ef 100644 --- a/examples/testbed/tests/box_stack.h +++ b/examples/testbed/tests/box_stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/capsule_collision.h b/examples/testbed/tests/capsule_collision.h index 8551b03..00a51ac 100644 --- a/examples/testbed/tests/capsule_collision.h +++ b/examples/testbed/tests/capsule_collision.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/capsule_spin.h b/examples/testbed/tests/capsule_spin.h index 0d6cc5a..94be411 100644 --- a/examples/testbed/tests/capsule_spin.h +++ b/examples/testbed/tests/capsule_spin.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/capsule_stack.h b/examples/testbed/tests/capsule_stack.h index f0462b8..d4b370c 100644 --- a/examples/testbed/tests/capsule_stack.h +++ b/examples/testbed/tests/capsule_stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/cloth_self_collision.h b/examples/testbed/tests/cloth_self_collision.h new file mode 100644 index 0000000..b363025 --- /dev/null +++ b/examples/testbed/tests/cloth_self_collision.h @@ -0,0 +1,140 @@ +/* +* 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 CLOTH_SELF_COLLISION_H +#define CLOTH_SELF_COLLISION_H + +class ClothSelfCollision : public Test +{ +public: + ClothSelfCollision() + { + // Translate the mesh + for (u32 i = 0; i < m_clothMesh.vertexCount; ++i) + { + m_clothMesh.vertices[i].y += 5.0f; + } + + // Create cloth + b3ClothDef def; + def.mesh = &m_clothMesh; + def.density = 1.0f; + def.streching = 100000.0f; + def.thickness = 0.2f; + def.friction = 0.3f; + + m_cloth = new b3Cloth(def); + + m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f)); + m_cloth->SetWorld(&m_world); + + { + b3BodyDef bd; + bd.type = e_staticBody; + + b3Body* b = m_world.CreateBody(bd); + + b3CapsuleShape capsuleShape; + capsuleShape.m_centers[0].Set(0.0f, 0.0f, -5.0f); + capsuleShape.m_centers[1].Set(0.0f, 0.0f, 5.0f); + capsuleShape.m_radius = 1.0f;; + + b3ShapeDef sd; + sd.shape = &capsuleShape; + sd.friction = 1.0f; + + b->CreateShape(sd); + } + + m_clothDragger = new b3ClothDragger(&m_ray, m_cloth); + } + + ~ClothSelfCollision() + { + delete m_cloth; + delete m_clothDragger; + } + + void Step() + { + Test::Step(); + + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); + + 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 ClothSelfCollision(); + } + + b3GridClothMesh<10, 10> m_clothMesh; + b3Cloth* m_cloth; + b3ClothDragger* m_clothDragger; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/cluster.h b/examples/testbed/tests/cluster.h index bd7b0b2..74bf446 100644 --- a/examples/testbed/tests/cluster.h +++ b/examples/testbed/tests/cluster.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/collide_test.h b/examples/testbed/tests/collide_test.h index 4818df3..f76d704 100644 --- a/examples/testbed/tests/collide_test.h +++ b/examples/testbed/tests/collide_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -50,13 +50,13 @@ public: g_draw->DrawSegment(pw, pw + wm.points[i].normal, b3Color_white); } - m_world.DrawShape(m_xfA, m_shapeA); - m_world.DrawShape(m_xfB, m_shapeB); + m_world.DrawShape(m_xfA, m_shapeA, b3Color_black); + m_world.DrawShape(m_xfB, m_shapeB, b3Color_black); g_draw->Flush(); - g_draw->DrawSolidShape(m_shapeA, b3Color(1.0f, 1.0f, 1.0f, 0.25f), m_xfA); - g_draw->DrawSolidShape(m_shapeB, b3Color(1.0f, 1.0f, 1.0f, 0.25f), m_xfB); + m_world.DrawSolidShape(m_xfA, m_shapeA, b3Color(1.0f, 1.0f, 1.0f, 0.25f)); + m_world.DrawSolidShape(m_xfB, m_shapeB, b3Color(1.0f, 1.0f, 1.0f, 0.25f)); g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow - Translate shape"); g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape"); diff --git a/examples/testbed/tests/compound_body.h b/examples/testbed/tests/compound_body.h index 0445d03..a1d1922 100644 --- a/examples/testbed/tests/compound_body.h +++ b/examples/testbed/tests/compound_body.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/cone_test.h b/examples/testbed/tests/cone_test.h index dfa6691..a75a598 100644 --- a/examples/testbed/tests/cone_test.h +++ b/examples/testbed/tests/cone_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/convex_hull.h b/examples/testbed/tests/convex_hull.h index a9b0e3c..1ddb27d 100644 --- a/examples/testbed/tests/convex_hull.h +++ b/examples/testbed/tests/convex_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/deep_capsule.h b/examples/testbed/tests/deep_capsule.h index c4cb36e..98eb010 100644 --- a/examples/testbed/tests/deep_capsule.h +++ b/examples/testbed/tests/deep_capsule.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/degenerate_capsule.h b/examples/testbed/tests/degenerate_capsule.h index 0ca046c..7a5b575 100644 --- a/examples/testbed/tests/degenerate_capsule.h +++ b/examples/testbed/tests/degenerate_capsule.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/distance_test.h b/examples/testbed/tests/distance_test.h index 44cbb20..4f659c7 100644 --- a/examples/testbed/tests/distance_test.h +++ b/examples/testbed/tests/distance_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -68,8 +68,8 @@ public: g_draw->DrawTransform(m_xfA); g_draw->DrawTransform(m_xfB); - m_world.DrawShape(m_xfA, &m_shapeA); - m_world.DrawShape(m_xfB, &m_shapeB); + m_world.DrawShape(m_xfA, &m_shapeA, b3Color_black); + m_world.DrawShape(m_xfB, &m_shapeB, b3Color_black); g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow - Translate shape"); g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape"); diff --git a/examples/testbed/tests/gyro_motion.h b/examples/testbed/tests/gyro_motion.h index cfbd897..2d35f1b 100644 --- a/examples/testbed/tests/gyro_motion.h +++ b/examples/testbed/tests/gyro_motion.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/hinge_chain.h b/examples/testbed/tests/hinge_chain.h index 3cc2e6b..7c0e297 100644 --- a/examples/testbed/tests/hinge_chain.h +++ b/examples/testbed/tests/hinge_chain.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/hinge_motor.h b/examples/testbed/tests/hinge_motor.h index 829f42b..0e8d01f 100644 --- a/examples/testbed/tests/hinge_motor.h +++ b/examples/testbed/tests/hinge_motor.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/hull_collision.h b/examples/testbed/tests/hull_collision.h index 8aad10e..1a24a06 100644 --- a/examples/testbed/tests/hull_collision.h +++ b/examples/testbed/tests/hull_collision.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/hull_contact_test.h b/examples/testbed/tests/hull_contact_test.h index 51fc123..186f387 100644 --- a/examples/testbed/tests/hull_contact_test.h +++ b/examples/testbed/tests/hull_contact_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/initial_overlap.h b/examples/testbed/tests/initial_overlap.h index b3e7156..fbfeec9 100644 --- a/examples/testbed/tests/initial_overlap.h +++ b/examples/testbed/tests/initial_overlap.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/jenga.h b/examples/testbed/tests/jenga.h index 8695def..7079d82 100644 --- a/examples/testbed/tests/jenga.h +++ b/examples/testbed/tests/jenga.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/linear_motion.h b/examples/testbed/tests/linear_motion.h index cd70d40..b94b5ac 100644 --- a/examples/testbed/tests/linear_motion.h +++ b/examples/testbed/tests/linear_motion.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/mass_spring.h b/examples/testbed/tests/mass_spring.h deleted file mode 100644 index 4c10924..0000000 --- a/examples/testbed/tests/mass_spring.h +++ /dev/null @@ -1,158 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 MASS_SPRING_H -#define MASS_SPRING_H - -class MassSpring : public Test -{ -public: - MassSpring() - { - m_x.Set(0.0f, 5.0f, 0.0f); - - m_v.SetZero(); - - m_k = 100.0f; - - m_iterations = 0; - } - - void Solve(float32 h) - { - // ODE - // f(Y) = dY / dt = [v] - // [-k * x] - - // 1. Apply Implicit Euler - - // Y(t + h) = Y(t) + h * f( Y(t + h) ) - // G( Y(t + h) ) = Y(t + h) - Y(t) - h * f( Y(t + h) ) = 0 - - // 2. Solve G = 0 - - // Newton-Raphson Iteration - // - // Y(t + h) = - // Y(t + h)_0 - G( Y(t + h)_0 ) / G'( Y(t + h)_0 ) = - // Y(t + h)_0 - G'( Y(t + h)_0 )^-1 * ( Y(t + h)_0 - Y(t) - h * f( Y(t + h)_0 ) - - // G'( Y ) = I - h * del_f / del_Y - - // del_f / del_Y = [del_f1 / del_x del_f1 / del_v] = [0 I] - // [del_f2 / del_x del_f2 / del_v] [-k * I 0] - - // G'( Y ) = [I 0] - [0 h * I] = [I -h * I] - // [0 I] [-h * k * I 0] [h * k * I I] - - // Compute Jacobian - b3Mat33 I = b3Mat33_identity; - - b3Mat33 A, B, C, D; - - A = I; - B = -h * I; - C = h * m_k * I; - D = I; - - // Invert - // Block matrix inversion - b3Mat33 invD = b3Inverse(D); - b3Mat33 B_invD = B * invD; - - b3Mat33 invJ_A = b3Inverse(A - B_invD * C); - b3Mat33 invJ_B = -invJ_A * B_invD; - b3Mat33 invJ_C = -invD * C * invJ_A; - b3Mat33 invJ_D = invD + invD * C * invJ_A * B_invD; - - // Initial guess - b3Vec3 f1 = m_v; - b3Vec3 f2 = -m_k * m_x; - - b3Vec3 Y1 = m_x + h * f1; - b3Vec3 Y2 = m_v + h * f2; - - const float32 kTol = 0.05f; - - const u32 kMaxIterations = 20; - - float32 eps0 = 0.0f; - - float32 eps1 = B3_MAX_FLOAT; - - m_iterations = 0; - - while (m_iterations < kMaxIterations && eps1 > kTol * kTol * eps0) - { - // Evaluate f(Y_n-1) - f1 = Y2; - f2 = -m_k * Y1; - - // Residual vector - b3Vec3 G1 = Y1 - m_x - h * f1; - b3Vec3 G2 = Y2 - m_v - h * f2; - - eps1 = b3Dot(G1, G1) + b3Dot(G2, G2); - - // Solve Ax = b - b3Vec3 x1 = invJ_A * G1 + invJ_B * G2; - b3Vec3 x2 = invJ_C * G1 + invJ_D * G2; - - Y1 -= x1; - Y2 -= x2; - - ++m_iterations; - } - - // Update state - m_x = Y1; - m_v = Y2; - } - - void Step() - { - float32 h = g_testSettings->inv_hertz; - - Solve(h); - - g_draw->DrawSolidSphere(m_x, 0.25f, b3Color_white); - - g_draw->DrawSegment(b3Vec3_zero, m_x, b3Color_white); - - g_draw->DrawString(b3Color_white, "Iterations = %u", m_iterations); - - float32 E = 0.5f * b3Dot(m_v, m_v); - g_draw->DrawString(b3Color_white, "E = %f", E); - } - - static Test* Create() - { - return new MassSpring(); - } - - // State - b3Vec3 m_x, m_v; - - // Stiffness - float32 m_k; - - // - u32 m_iterations; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/mesh_contact_test.h b/examples/testbed/tests/mesh_contact_test.h index d81f05f..e68dd41 100644 --- a/examples/testbed/tests/mesh_contact_test.h +++ b/examples/testbed/tests/mesh_contact_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -24,6 +24,8 @@ class MeshContactTest : public Test public: MeshContactTest() { + m_gridMesh.BuildTree(); + // Transform grid into a terrain for (u32 i = 0; i < m_terrainMesh.vertexCount; ++i) { @@ -37,7 +39,7 @@ public: m_ground = m_world.CreateBody(bd); b3MeshShape ms; - ms.m_mesh = &m_groundMesh; + ms.m_mesh = &m_gridMesh; b3ShapeDef sd; sd.shape = &ms; @@ -138,7 +140,7 @@ public: if (key == GLFW_KEY_G) { b3MeshShape ms; - ms.m_mesh = &m_groundMesh; + ms.m_mesh = &m_gridMesh; b3ShapeDef sd; sd.shape = &ms; @@ -176,7 +178,8 @@ public: } b3GridMesh<25, 25> m_terrainMesh; - + b3GridMesh<25, 25> m_gridMesh; + b3Body* m_ground; b3Body* m_body; }; diff --git a/examples/testbed/tests/multiple_pendulum.h b/examples/testbed/tests/multiple_pendulum.h index 487b73f..6bc43b7 100644 --- a/examples/testbed/tests/multiple_pendulum.h +++ b/examples/testbed/tests/multiple_pendulum.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/newton_cradle.h b/examples/testbed/tests/newton_cradle.h index 445cd1c..865cb3a 100644 --- a/examples/testbed/tests/newton_cradle.h +++ b/examples/testbed/tests/newton_cradle.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/particle_types.h b/examples/testbed/tests/particle_types.h index 5aa4c71..55f51f6 100644 --- a/examples/testbed/tests/particle_types.h +++ b/examples/testbed/tests/particle_types.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index dfafeab..e856ac5 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,32 +19,23 @@ #ifndef PINNED_CLOTH_H #define PINNED_CLOTH_H -class PinnedCloth : public ClothTest +class PinnedCloth : public Test { public: - PinnedCloth() : m_rectangleGarment(5.0f, 5.0f) + PinnedCloth() { - // Generate 2D mesh - m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - - // Create 3D mesh - m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - - // - b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); - for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) - { - m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; - } - + // Create cloth b3ClothDef def; - def.mesh = &m_rectangleClothMesh; + def.mesh = &m_clothMesh; def.density = 0.2f; - def.structural = 100000.0f; - def.damping = 0.0f; + def.streching = 100000.0f; - m_cloth = m_world.CreateCloth(def); + m_cloth = new b3Cloth(def); + m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f)); + m_cloth->SetWorld(&m_world); + + // Freeze some particles b3AABB3 aabb1; aabb1.m_lower.Set(-5.0f, -1.0f, -6.0f); aabb1.m_upper.Set(5.0f, 1.0f, -4.0f); @@ -65,23 +56,70 @@ public: p->SetType(e_staticParticle); } } + + m_clothDragger = new b3ClothDragger(&m_ray, m_cloth); + } + ~PinnedCloth() + { + delete m_clothDragger; + delete m_cloth; + } + + void Step() + { + Test::Step(); + + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); + + m_cloth->Draw(); + + if (m_clothDragger->IsDragging()) { - b3BodyDef bd; - bd.type = e_dynamicBody; - bd.position.Set(0.0f, 5.0f, 0.0f); + b3Vec3 pA = m_clothDragger->GetPointA(); + b3Vec3 pB = m_clothDragger->GetPointB(); - b3Body* b = m_world.CreateBody(bd); + g_draw->DrawPoint(pA, 2.0f, b3Color_green); - b3SphereShape sphere; - sphere.m_center.SetZero(); - sphere.m_radius = 2.0f; + g_draw->DrawPoint(pB, 2.0f, b3Color_green); - b3ShapeDef sd; - sd.shape = &sphere; - sd.density = 1.0f; + g_draw->DrawSegment(pA, pB, b3Color_white); + } - b3Shape* s = b->CreateShape(sd); + 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(); } } @@ -90,9 +128,9 @@ public: return new PinnedCloth(); } - b3RectangleGarment m_rectangleGarment; - b3GarmentMesh m_rectangleGarmentMesh; - b3GarmentClothMesh m_rectangleClothMesh; + b3GridClothMesh<10, 10> m_clothMesh; + b3Cloth* m_cloth; + b3ClothDragger* m_clothDragger; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/pinned_softbody.h b/examples/testbed/tests/pinned_softbody.h new file mode 100644 index 0000000..e058e24 --- /dev/null +++ b/examples/testbed/tests/pinned_softbody.h @@ -0,0 +1,144 @@ +/* +* 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 + +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 = 1000.0f; + def.nu = 0.33f; + def.c_yield = 0.1f; + def.c_creep = 0.5f; + def.c_max = 1.0f; + + m_body = new b3SoftBody(def); + + for (u32 i = 0; i < m_mesh.vertexCount; ++i) + { + b3SoftBodyNode* n = m_body->GetVertexNode(i); + n->SetMassDamping(0.2f); + } + + 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 \ No newline at end of file diff --git a/examples/testbed/tests/point_click.h b/examples/testbed/tests/point_click.h deleted file mode 100644 index 0f1f3a1..0000000 --- a/examples/testbed/tests/point_click.h +++ /dev/null @@ -1,98 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 POINT_CLICK_H -#define POINT_CLICK_H - -class PointClick : public Test -{ -public: - PointClick() - { - { - b3BodyDef bdef; - b3Body* ground = m_world.CreateBody(bdef); - - b3MeshShape ms; - ms.m_mesh = &m_groundMesh; - - b3ShapeDef sd; - sd.shape = &ms; - - ground->CreateShape(sd); - } - - { - b3BodyDef bdef; - bdef.type = b3BodyType::e_dynamicBody; - bdef.fixedRotationY = true; - bdef.position.Set(0.0f, 5.0f, 0.0f); - - m_character = m_world.CreateBody(bdef); - - b3CapsuleShape cap; - cap.m_centers[0].Set(0.0f, 1.0f, 0.0f); - cap.m_centers[1].Set(0.0f, -1.0f, 0.0f); - cap.m_radius = 1.0f; - - b3ShapeDef sdef; - sdef.shape = ∩ - sdef.density = 1.0f; - sdef.friction = 0.5f; - - m_character->CreateShape(sdef); - } - } - - void BeginDragging() - { - if (m_bodyDragger.GetBody() == m_character) - { - m_bodyDragger.StopDragging(); - } - } - - void Step() - { - if (m_bodyDragger.IsDragging()) - { - if (m_bodyDragger.GetBody() != m_character) - { - b3Vec3 p1 = m_character->GetPosition(); - b3Vec3 p2 = m_bodyDragger.GetPointA(); - - b3Vec3 n = b3Normalize(p2 - p1); - const float32 k = 1000.0f; - b3Vec3 f = k * n; - - m_character->ApplyForceToCenter(f, true); - } - } - - Test::Step(); - } - - static Test* Create() - { - return new PointClick(); - } - - b3Body* m_character; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/pyramid.h b/examples/testbed/tests/pyramid.h index ecb3235..d7528e3 100644 --- a/examples/testbed/tests/pyramid.h +++ b/examples/testbed/tests/pyramid.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/pyramids.h b/examples/testbed/tests/pyramids.h index 60f4cff..e440e9d 100644 --- a/examples/testbed/tests/pyramids.h +++ b/examples/testbed/tests/pyramids.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/quadric_shapes.h b/examples/testbed/tests/quadric_shapes.h index 8bb1395..f00133a 100644 --- a/examples/testbed/tests/quadric_shapes.h +++ b/examples/testbed/tests/quadric_shapes.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/ragdoll.h b/examples/testbed/tests/ragdoll.h index 4942ebb..5dac8cb 100644 --- a/examples/testbed/tests/ragdoll.h +++ b/examples/testbed/tests/ragdoll.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/ray_cast.h b/examples/testbed/tests/ray_cast.h index 3b7e7ed..579bdd8 100644 --- a/examples/testbed/tests/ray_cast.h +++ b/examples/testbed/tests/ray_cast.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -178,8 +178,8 @@ public: void CastRay(const b3Vec3 p1, const b3Vec3 p2) const { - b3RayCastSingleShapeOutput out; - if (m_world.RayCastSingleShape(&out, p1, p2)) + b3RayCastSingleOutput out; + if (m_world.RayCastSingle(&out, p1, p2)) { g_draw->DrawSegment(p1, out.point, b3Color_green); diff --git a/examples/testbed/tests/rope_test.h b/examples/testbed/tests/rope_test.h index de9252d..389f517 100644 --- a/examples/testbed/tests/rope_test.h +++ b/examples/testbed/tests/rope_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 hebd liable for any damages diff --git a/examples/testbed/tests/self_collision.h b/examples/testbed/tests/self_collision.h deleted file mode 100644 index 9a3ed52..0000000 --- a/examples/testbed/tests/self_collision.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 SELF_COLLISION_H -#define SELF_COLLISION_H - -class SelfCollision : public ClothTest -{ -public: - SelfCollision() : m_rectangleGarment(5.0f, 5.0f) - { - // Generate 2D mesh - m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - - // Create 3D mesh - m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - - b3Mat33 Rx = b3Mat33RotationX(0.5f * B3_PI); - for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) - { - m_rectangleClothMesh.vertices[i] = Rx * m_rectangleClothMesh.vertices[i]; - m_rectangleClothMesh.vertices[i].y += 5.0f; - } - - b3ClothDef def; - def.mesh = &m_rectangleClothMesh; - def.density = 1.0f; - def.structural = 100000.0f; - - m_cloth = m_world.CreateCloth(def); - - for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) - { - p->SetRadius(0.2f); - p->SetFriction(0.2f); - } - - { - b3BodyDef bd; - bd.type = e_staticBody; - - b3Body* b = m_world.CreateBody(bd); - - b3CapsuleShape capsuleShape; - capsuleShape.m_centers[0].Set(0.0f, 0.0f, -5.0f); - capsuleShape.m_centers[1].Set(0.0f, 0.0f, 5.0f); - capsuleShape.m_radius = 1.0f;; - - b3ShapeDef sd; - sd.shape = &capsuleShape; - sd.friction = 1.0f; - - b->CreateShape(sd); - } - } - - static Test* Create() - { - return new SelfCollision(); - } - - b3RectangleGarment m_rectangleGarment; - b3GarmentMesh m_rectangleGarmentMesh; - b3GarmentClothMesh m_rectangleClothMesh; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/sensor_test.h b/examples/testbed/tests/sensor_test.h index 63bc2c4..3d8e64f 100644 --- a/examples/testbed/tests/sensor_test.h +++ b/examples/testbed/tests/sensor_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/shape_cast.h b/examples/testbed/tests/shape_cast.h new file mode 100644 index 0000000..f08e083 --- /dev/null +++ b/examples/testbed/tests/shape_cast.h @@ -0,0 +1,121 @@ +#ifndef SHAPE_CAST_H +#define SHAPE_CAST_H + +class ShapeCast : public Test +{ +public: + ShapeCast() + { + m_shapeA.m_hull = &b3BoxHull_identity; + m_shapeA.m_radius = 0.0f; + + m_shapeB.m_hull = &b3BoxHull_identity; + m_shapeB.m_radius = 0.0f; + + m_xfA.position.Set(-5.0f, 0.0f, 0.0f); + m_xfA.rotation.SetIdentity(); + + m_xfB.position.Set(10.0f, 0.0f, 0.0f); + m_xfB.rotation.SetIdentity(); + + m_proxyA.Set(&m_shapeA, 0); + m_proxyB.Set(&m_shapeB, 0); + } + + void Step() + { + g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow - Translate shape"); + g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape"); + + g_draw->DrawTransform(m_xfA); + g_draw->DrawTransform(m_xfB); + + m_world.DrawShape(m_xfA, &m_shapeA, b3Color_black); + m_world.DrawShape(m_xfB, &m_shapeB, b3Color_black); + + m_world.DrawSolidShape(m_xfA, &m_shapeA, b3Color_white); + m_world.DrawSolidShape(m_xfB, &m_shapeB, b3Color_white); + + b3Vec3 translationB = -100.0f * b3Vec3_x; + g_draw->DrawSegment(m_xfB.position, m_xfB.position + translationB, b3Color_white); + + b3GJKShapeCastOutput out; + bool hit = b3GJKShapeCast(&out, m_xfA, m_proxyA, m_xfB, m_proxyB, translationB); + + g_draw->DrawString(b3Color_white, "Iterations = %d", out.iterations); + + if (hit) + { + g_draw->DrawPoint(out.point, 4.0f, b3Color_green); + g_draw->DrawSegment(out.point, out.point + out.normal, b3Color_green); + + b3Transform xfB; + xfB.rotation = m_xfB.rotation; + xfB.position = m_xfB.position + out.t * translationB; + + m_world.DrawShape(xfB, &m_shapeB, b3Color_black); + } + } + + void KeyDown(int key) + { + if (key == GLFW_KEY_LEFT) + { + m_xfB.position.x -= 0.105f; + } + + if (key == GLFW_KEY_RIGHT) + { + m_xfB.position.x += 0.105f; + } + + if (key == GLFW_KEY_UP) + { + m_xfB.position.y += 0.105f; + } + + if (key == GLFW_KEY_DOWN) + { + m_xfB.position.y -= 0.105f; + } + + if (key == GLFW_KEY_X) + { + b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI); + b3Mat33 xfx = b3QuatMat33(qx); + + m_xfB.rotation = m_xfB.rotation * xfx; + } + + if (key == GLFW_KEY_Y) + { + b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI); + b3Mat33 xfy = b3QuatMat33(qy); + + m_xfB.rotation = m_xfB.rotation * xfy; + } + + if (key == GLFW_KEY_Z) + { + b3Quat qy(b3Vec3(0.0f, 0.0f, 1.0f), 0.05f * B3_PI); + b3Mat33 xfz = b3QuatMat33(qy); + + m_xfB.rotation = m_xfB.rotation * xfz; + } + } + + static Test* Create() + { + return new ShapeCast(); + } + + b3HullShape m_shapeA; + b3Transform m_xfA; + b3ShapeGJKProxy m_proxyA; + + b3HullShape m_shapeB; + b3Transform m_xfB; + b3ShapeGJKProxy m_proxyB; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/shape_stack.h b/examples/testbed/tests/shape_stack.h index a22b6ba..8d6097c 100644 --- a/examples/testbed/tests/shape_stack.h +++ b/examples/testbed/tests/shape_stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/sheet_stack.h b/examples/testbed/tests/sheet_stack.h index afdbba9..7a32e93 100644 --- a/examples/testbed/tests/sheet_stack.h +++ b/examples/testbed/tests/sheet_stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/shirt.h b/examples/testbed/tests/shirt.h deleted file mode 100644 index 158b8f1..0000000 --- a/examples/testbed/tests/shirt.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 SHIRT_H -#define SHIRT_H - -class Shirt : public ClothTest -{ -public: - Shirt() - { - // Generate 2D mesh - m_shirtGarmentMesh.Set(&m_shirtGarment, 0.1f); - - // Create 3D mesh - m_shirtClothMesh.Set(&m_shirtGarmentMesh); - - // Perform fitting - for (u32 i = 0; i < 3; ++i) - { - b3ClothMeshMesh* front = m_shirtClothMesh.meshes + i; - for (u32 j = 0; j < front->vertexCount; ++j) - { - u32 v = front->startVertex + j; - m_shirtClothMesh.vertices[v].z = -1.0f; - } - } - - for (u32 i = 3; i < 6; ++i) - { - b3ClothMeshMesh* back = m_shirtClothMesh.meshes + i; - for (u32 j = 0; j < back->vertexCount; ++j) - { - u32 v = back->startVertex + j; - m_shirtClothMesh.vertices[v].z = 1.0f; - } - } - - // Create cloth - b3ClothDef def; - def.mesh = &m_shirtClothMesh; - def.density = 0.2f; - def.structural = 10000.0f; - - m_cloth = m_world.CreateCloth(def); - } - - static Test* Create() - { - return new Shirt(); - } - - b3ShirtGarment m_shirtGarment; - b3GarmentMesh m_shirtGarmentMesh; - b3GarmentClothMesh m_shirtClothMesh; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/single_pendulum.h b/examples/testbed/tests/single_pendulum.h deleted file mode 100644 index 4746d3e..0000000 --- a/examples/testbed/tests/single_pendulum.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 PENDULUM_H -#define PENDULUM_H - -class SinglePendulum : public Test -{ -public: - SinglePendulum() - { - m_g = -10.0f; - - m_r = 10.0f; - m_m = 1.0f; - m_I = m_m * m_r * m_r; - - // Initial state - m_theta = -0.5f * B3_PI; - m_omega = 0.0f; - } - - void Step() - { - float32 h = g_testSettings->inv_hertz; - - // Solution (acceleration) - float32 omega_dot = -m_g / m_r * sin(m_theta); - - // Integrate acceleration - m_omega += h * omega_dot; - - // Integrate velocity - m_theta += h * m_omega; - - // Convert from polar coordinates (r, theta) to Cartesian coordinates (x, y) - b3Vec3 c; - c.x = m_r * sin(m_theta); - c.y = m_r * cos(m_theta); - c.z = 0.0f; - g_draw->DrawSolidSphere(c, 1.0f, b3Color_white); - - b3Vec3 pole; - pole.SetZero(); - g_draw->DrawSegment(pole, c, b3Color_white); - - // Kinetic energy - float32 T = 0.5f * m_I * m_omega * m_omega; - - // Potential energy - float32 V = -m_m * m_g * m_r * cos(m_theta); - - // Lagrangian - float32 L = T - V; - - // - g_draw->DrawString(b3Color_white, "T = %f \nV = %f \nL = %f", T, V, L); - } - - static Test* Create() - { - return new SinglePendulum(); - } - - // Gravity - float32 m_g; - - // Mass, inertia - float32 m_m, m_I; - - // Radial coordinate - float32 m_r; - - // The allowable generalized coordinate in polar coordinate frame. - // Only motions satisfying the constraints can be described - // in this frame. Therefore, all solutions satisfy the constraints. - // This is the so called reduced coordinates approach. - float32 m_theta; - - // Velocity - float32 m_omega; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/smash_softbody.h b/examples/testbed/tests/smash_softbody.h new file mode 100644 index 0000000..544d5a2 --- /dev/null +++ b/examples/testbed/tests/smash_softbody.h @@ -0,0 +1,175 @@ +/* +* 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 + +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; + def.c_yield = 0.6f; + def.c_creep = 1.0f; + def.c_max = 1.0f; + + 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 \ No newline at end of file diff --git a/examples/testbed/tests/sphere_stack.h b/examples/testbed/tests/sphere_stack.h index 1800e56..b5f48b6 100644 --- a/examples/testbed/tests/sphere_stack.h +++ b/examples/testbed/tests/sphere_stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/spring.h b/examples/testbed/tests/spring.h index 4d4f8be..ad5df0b 100644 --- a/examples/testbed/tests/spring.h +++ b/examples/testbed/tests/spring.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index c0ec318..9460ffe 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,39 +19,31 @@ #ifndef TABLE_CLOTH_H #define TABLE_CLOTH_H -class TableCloth : public ClothTest +class TableCloth : public Test { public: - TableCloth() : m_rectangleGarment(5.0f, 5.0f) + TableCloth() { - // Generate 2D mesh - m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - - // Create 3D mesh - m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - - // - b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); - for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) + // Translate the mesh + for (u32 i = 0; i < m_clothMesh.vertexCount; ++i) { - m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; - m_rectangleClothMesh.vertices[i].y += 5.0f; + m_clothMesh.vertices[i].y += 5.0f; } - + + // Create cloth b3ClothDef def; - def.mesh = &m_rectangleClothMesh; + def.mesh = &m_clothMesh; def.density = 0.2f; - def.bending = 10000.0f; - def.structural = 10000.0f; - def.damping = 0.0f; + def.streching = 10000.0f; + //def.shearing = 10000.0f; + def.damping = 100.0f; + def.thickness = 0.2f; + def.friction = 0.1f; - m_cloth = m_world.CreateCloth(def); + m_cloth = new b3Cloth(def); - for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) - { - p->SetRadius(0.2f); - p->SetFriction(0.2f); - } + m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f)); + m_cloth->SetWorld(&m_world); { b3BodyDef bd; @@ -63,7 +55,6 @@ public: b3HullShape tableShape; tableShape.m_hull = &m_tableHull; - tableShape.m_radius = 0.2f; b3ShapeDef sd; sd.shape = &tableShape; @@ -71,6 +62,71 @@ public: b->CreateShape(sd); } + + m_clothDragger = new b3ClothDragger(&m_ray, m_cloth); + } + + ~TableCloth() + { + delete m_clothDragger; + delete m_cloth; + } + + void Step() + { + Test::Step(); + + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); + + 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() @@ -78,10 +134,10 @@ public: return new TableCloth(); } - b3RectangleGarment m_rectangleGarment; - b3GarmentMesh m_rectangleGarmentMesh; - b3GarmentClothMesh m_rectangleClothMesh; - + b3GridClothMesh<10, 10> m_clothMesh; + b3Cloth* m_cloth; + b3ClothDragger* m_clothDragger; + b3QHull m_tableHull; }; diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index 463113d..c328fd5 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -55,31 +55,25 @@ static inline b3Color Color(float32 x, float32 a, float32 b) return c; } -class TensionMapping : public ClothTest +class TensionMapping : public Test { public: - TensionMapping() : m_rectangleGarment(5.0f, 5.0f) + TensionMapping() { - // Generate 2D mesh - m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - - // Create 3D mesh - m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - - // - b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); - for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) - { - m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; - } - + // Create cloth b3ClothDef def; - def.mesh = &m_rectangleClothMesh; + def.mesh = &m_clothMesh; def.density = 0.2f; - def.structural = 10000.0f; + def.streching = 10000.0f; + def.shearing = 5000.0f; + def.damping = 100.0f; - m_cloth = m_world.CreateCloth(def); + m_cloth = new b3Cloth(def); + m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f)); + m_cloth->SetWorld(&m_world); + + // Freeze some particles b3AABB3 aabb; aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); aabb.m_upper.Set(5.0f, 1.0f, -4.0f); @@ -91,12 +85,22 @@ public: p->SetType(e_staticParticle); } } + + m_clothDragger = new b3ClothDragger(&m_ray, m_cloth); + } + + ~TensionMapping() + { + delete m_clothDragger; + delete m_cloth; } void Step() { Test::Step(); + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); + const b3ClothMesh* mesh = m_cloth->GetMesh(); b3StackArray tension; @@ -108,29 +112,45 @@ public: for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext()) { - if (f->GetType() == e_springForce) + if (f->GetType() == e_strechForce) { - b3SpringForce* s = (b3SpringForce*)f; + b3StrechForce* s = (b3StrechForce*)f; + + b3ClothTriangle* triangle = s->GetTriangle(); + u32 triangleIndex = triangle->GetTriangle(); + b3ClothMeshTriangle* mesh_triangle = m_clothMesh.triangles + triangleIndex; + + u32 v1 = mesh_triangle->v1; + u32 v2 = mesh_triangle->v2; + u32 v3 = mesh_triangle->v3; - u32 v1 = s->GetParticle1()->GetVertex(); - u32 v2 = s->GetParticle2()->GetVertex(); + b3Vec3 f1 = s->GetActionForce1(); + b3Vec3 f2 = s->GetActionForce2(); + b3Vec3 f3 = s->GetActionForce3(); - tension[v1] += s->GetActionForce(); - tension[v2] -= s->GetActionForce(); + tension[v1] += f1; + tension[v2] += f2; + tension[v3] += f3; } } - for (u32 i = 0; i < m_rectangleClothMesh.triangleCount; ++i) + for (u32 i = 0; i < mesh->triangleCount; ++i) { - b3ClothMeshTriangle* t = m_rectangleClothMesh.triangles + i; + b3ClothMeshTriangle* t = mesh->triangles + i; - b3Vec3 v1 = m_cloth->GetVertexParticle(t->v1)->GetPosition(); - b3Vec3 v2 = m_cloth->GetVertexParticle(t->v2)->GetPosition(); - b3Vec3 v3 = m_cloth->GetVertexParticle(t->v3)->GetPosition(); + b3Vec3 v1 = m_cloth->GetParticle(t->v1)->GetPosition(); + b3Vec3 v2 = m_cloth->GetParticle(t->v2)->GetPosition(); + b3Vec3 v3 = m_cloth->GetParticle(t->v3)->GetPosition(); + + g_draw->DrawTriangle(v1, v2, v3, b3Color_black); - b3Draw_draw->DrawSegment(v1, v2, b3Color_black); - b3Draw_draw->DrawSegment(v2, v3, b3Color_black); - b3Draw_draw->DrawSegment(v3, v1, b3Color_black); + b3Vec3 c = (v1 + v2 + v3) / 3.0f; + + float32 s = 0.9f; + + v1 = s * (v1 - c) + c; + v2 = s * (v2 - c) + c; + v3 = s * (v3 - c) + c; b3Vec3 f1 = tension[t->v1]; float32 L1 = b3Length(f1); @@ -143,7 +163,7 @@ public: float32 L = (L1 + L2 + L3) / 3.0f; - const float32 kMaxT = 100000.0f; + const float32 kMaxT = 10000.0f; b3Color color = Color(L, 0.0f, kMaxT); b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); @@ -151,29 +171,66 @@ public: g_draw->DrawSolidTriangle(n1, v1, v2, v3, color); b3Vec3 n2 = -n1; - g_draw->DrawSolidTriangle(n2, v1, v3, v2, color); + g_draw->DrawSolidTriangle(n2, v3, v2, v1, color); } - if (m_clothDragger.IsDragging() == true) + if (m_clothDragger->IsDragging()) { - g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white); + 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 = %u", 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 TensionMapping(); } - b3RectangleGarment m_rectangleGarment; - b3GarmentMesh m_rectangleGarmentMesh; - b3GarmentClothMesh m_rectangleClothMesh; + b3GridClothMesh<10, 10> m_clothMesh; + b3Cloth* m_cloth; + b3ClothDragger* m_clothDragger; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/tumbler.h b/examples/testbed/tests/tumbler.h index 9f79bf0..471fb2c 100644 --- a/examples/testbed/tests/tumbler.h +++ b/examples/testbed/tests/tumbler.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/varying_friction.h b/examples/testbed/tests/varying_friction.h index 0a1c2ab..30726a7 100644 --- a/examples/testbed/tests/varying_friction.h +++ b/examples/testbed/tests/varying_friction.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/varying_restitution.h b/examples/testbed/tests/varying_restitution.h index d6ffeb6..b25e9fc 100644 --- a/examples/testbed/tests/varying_restitution.h +++ b/examples/testbed/tests/varying_restitution.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/examples/testbed/tests/weld_test.h b/examples/testbed/tests/weld_test.h index b35c825..1ca7aac 100644 --- a/examples/testbed/tests/weld_test.h +++ b/examples/testbed/tests/weld_test.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 1c4162f..3826bb5 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -56,23 +56,32 @@ #include #include -#include - -#include -#include -#include - -#include -#include -#include -#include - #include #include #include -#include -#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include #endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/cloth/cloth.h similarity index 64% rename from include/bounce/dynamics/cloth/cloth.h rename to include/bounce/cloth/cloth.h index 6f2d6f5..9419b23 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,21 +19,21 @@ #ifndef B3_CLOTH_H #define B3_CLOTH_H -#include #include +#include #include +#include +#include class b3World; -class b3Shape; - -class b3Particle; -class b3Force; -class b3BodyContact; -class b3ParticleContact; -class b3TriangleContact; struct b3ParticleDef; +class b3Particle; + struct b3ForceDef; +class b3Force; + +class b3ClothTriangle; struct b3ClothMesh; @@ -57,33 +57,62 @@ struct b3ClothDef { mesh = nullptr; density = 0.0f; - structural = 0.0f; + streching = 0.0f; + shearing = 0.0f; bending = 0.0f; + sewing = 0.0f; damping = 0.0f; + thickness = 0.0f; + friction = 0.2f; } // Cloth mesh const b3ClothMesh* mesh; - // Cloth density in kg/m^3 + // Cloth density in kg/m^2 float32 density; - // Structural stiffness - float32 structural; + // Streching stiffness + float32 streching; + // Shearing stiffness + float32 shearing; + // Bending stiffness float32 bending; + // Sewing stiffness + float32 sewing; + // Damping stiffness float32 damping; + + // Cloth thickness + float32 thickness; + + // Cloth coefficient of friction + float32 friction; }; // A cloth represents a deformable surface as a collection of particles. -// Particles may be connected with each other. +// Particles may be connected with each other by springs. class b3Cloth { public: - // Get the world the cloth belongs to. + b3Cloth(const b3ClothDef& def); + ~b3Cloth(); + + // 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 rigid bodies in the attached world. + void SetWorld(b3World* world); + + // Get the world attached to this cloth. const b3World* GetWorld() const; b3World* GetWorld(); @@ -99,9 +128,6 @@ public: // Destroy a given force. void DestroyForce(b3Force* force); - // Perform a ray cast with the cloth. - void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2); - // Perform a ray cast with the cloth. bool RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const; @@ -111,8 +137,11 @@ public: // Return the cloth mesh proxy. const b3ClothMesh* GetMesh() const; - // Return the particle associated with the given vertex. - b3Particle* GetVertexParticle(u32 i); + // Return the cloth particle given the vertex index. + b3Particle* GetParticle(u32 i); + + // Return the cloth triangle given the triangle index. + b3ClothTriangle* GetTriangle(u32 i); // Return the list of particles in this cloth. const b3List2& GetParticleList() const; @@ -123,49 +152,43 @@ public: // Return the kinetic (or dynamic) energy in this system. float32 GetEnergy() const; - // Get the next cloth in the world cloth list. - const b3Cloth* GetNext() const; - - // Get the next cloth in the world cloth list. - b3Cloth* GetNext(); + // Perform a time step. + void Step(float32 dt, u32 velocityIterations, u32 positionIterations); // Debug draw the cloth using the associated cloth mesh. void Draw() const; private: - friend class b3World; - - friend class b3List2; - - b3Cloth(const b3ClothDef& def, b3World* world); - ~b3Cloth(); - - // Perform a time step. - // Called only inside b3World. - void Step(float32 dt, const b3Vec3& gravity); + friend class b3Particle; + friend class b3ClothTriangle; + friend class b3ShearForce; + friend class b3StrechForce; + friend class b3SpringForce; + friend class b3MouseForce; + friend class b3ClothContactManager; // Compute mass of each particle. void ComputeMass(); - // Update body contacts. - void UpdateBodyContacts(); - - // Update particle contacts. - void UpdateParticleContacts(); - - // Update triangle contacts. - void UpdateTriangleContacts(); - - // Update contacts - void UpdateContacts(); - // Solve - void Solve(float32 dt, const b3Vec3& gravity); + void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); + + // Stack allocator + b3StackAllocator m_stackAllocator; + + // The world attached to this cloth + b3World* m_world; + + // Gravity acceleration + b3Vec3 m_gravity; // Proxy mesh const b3ClothMesh* m_mesh; - // Vertex particles - b3Particle** m_vertexParticles; + // Particles + b3Particle** m_particles; + + // Triangles + b3ClothTriangle* m_triangles; // Cloth density float32 m_density; @@ -173,38 +196,26 @@ private: // Pool of particles b3BlockPool m_particleBlocks; - // Pool of body contacts - b3BlockPool m_bodyContactBlocks; - - // Pool of particle contacts - b3BlockPool m_particleContactBlocks; - - // Pool of triangle contacts - b3BlockPool m_triangleContactBlocks; - // List of particles b3List2 m_particleList; - + // List of forces b3List2 m_forceList; - // List of particle contacts - b3List2 m_bodyContactList; - - // List of particle contacts - b3List2 m_particleContactList; - - // List of triangle contacts - b3List2 m_triangleContactList; - - // The parent world of this cloth. - b3World* m_world; - - // Links to the world cloth list. - b3Cloth* m_prev; - b3Cloth* m_next; + // Contact manager + b3ClothContactManager m_contactManager; }; +inline void b3Cloth::SetGravity(const b3Vec3& gravity) +{ + m_gravity = gravity; +} + +inline b3Vec3 b3Cloth::GetGravity() const +{ + return m_gravity; +} + inline const b3World* b3Cloth::GetWorld() const { return m_world; @@ -230,14 +241,4 @@ inline const b3List2& b3Cloth::GetForceList() const return m_forceList; } -inline const b3Cloth* b3Cloth::GetNext() const -{ - return m_next; -} - -inline b3Cloth* b3Cloth::GetNext() -{ - return m_next; -} - #endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_collision.h b/include/bounce/cloth/cloth_collision.h new file mode 100644 index 0000000..58335f5 --- /dev/null +++ b/include/bounce/cloth/cloth_collision.h @@ -0,0 +1,38 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_COLLISION_H +#define B3_CLOTH_COLLISION_H + +#include + +// Cloth primitive type +enum b3ClothAABBProxyType +{ + e_particleProxy, + e_triangleProxy +}; + +// Cloth primitive broadphase proxy +struct b3ClothAABBProxy +{ + b3ClothAABBProxyType type; + void* owner; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_contact_manager.h b/include/bounce/cloth/cloth_contact_manager.h new file mode 100644 index 0000000..c1e8bd7 --- /dev/null +++ b/include/bounce/cloth/cloth_contact_manager.h @@ -0,0 +1,63 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_CONTACT_MANAGER_H +#define B3_CLOTH_CONTACT_MANAGER_H + +#include +#include +#include +#include +#include + +class b3Cloth; + +// Contact delegator for b3Cloth. +class b3ClothContactManager +{ +public: + b3ClothContactManager(); + + void FindNewContacts(); + + void AddPair(void* data1, void* data2); + void FindNewClothContacts(); + + void AddPSPair(b3Particle* p1, b3Shape* s2); + void FindNewBodyContacts(); + + void UpdateContacts(); + void UpdateClothContacts(); + void UpdateBodyContacts(); + + b3ParticleTriangleContact* CreateParticleTriangleContact(); + void Destroy(b3ParticleTriangleContact* c); + + b3ParticleBodyContact* CreateParticleBodyContact(); + void Destroy(b3ParticleBodyContact* c); + + b3BlockPool m_particleTriangleContactBlocks; + b3BlockPool m_particleBodyContactBlocks; + + b3Cloth* m_cloth; + b3BroadPhase m_broadPhase; + b3List2 m_particleTriangleContactList; + b3List2 m_particleBodyContactList; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_force_solver.h b/include/bounce/cloth/cloth_force_solver.h new file mode 100644 index 0000000..2762c7f --- /dev/null +++ b/include/bounce/cloth/cloth_force_solver.h @@ -0,0 +1,77 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_FORCE_SOLVER_H +#define B3_CLOTH_FORCE_SOLVER_H + +#include +#include + +class b3StackAllocator; + +class b3Particle; +class b3Force; + +struct b3DenseVec3; +struct b3DiagMat33; +struct b3SparseMat33; +struct b3SparseMat33View; + +struct b3ClothForceSolverDef +{ + b3StackAllocator* stack; + u32 particleCount; + b3Particle** particles; + u32 forceCount; + b3Force** forces; +}; + +struct b3ClothForceSolverData +{ + b3DenseVec3* x; + b3DenseVec3* v; + b3DenseVec3* f; + b3DenseVec3* y; + b3SparseMat33* dfdx; + b3SparseMat33* dfdv; + b3DiagMat33* S; + b3DenseVec3* z; +}; + +class b3ClothForceSolver +{ +public: + b3ClothForceSolver(const b3ClothForceSolverDef& def); + ~b3ClothForceSolver(); + + void Solve(float32 dt, const b3Vec3& gravity); +private: + void ApplyForces(); + + b3StackAllocator* m_allocator; + + u32 m_particleCount; + b3Particle** m_particles; + + u32 m_forceCount; + b3Force** m_forces; + + b3ClothForceSolverData m_solverData; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth_mesh.h b/include/bounce/cloth/cloth_mesh.h similarity index 80% rename from include/bounce/dynamics/cloth/cloth_mesh.h rename to include/bounce/cloth/cloth_mesh.h index 593f6dc..98c9205 100644 --- a/include/bounce/dynamics/cloth/cloth_mesh.h +++ b/include/bounce/cloth/cloth_mesh.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,7 +19,6 @@ #ifndef B3_CLOTH_MESH_H #define B3_CLOTH_MESH_H -#include #include struct b3ClothMeshTriangle @@ -53,16 +52,4 @@ struct b3ClothMesh b3ClothMeshSewingLine* sewingLines; }; -struct b3GarmentMesh; - -// Convenience structure. -struct b3GarmentClothMesh : public b3ClothMesh -{ - b3GarmentClothMesh(); - ~b3GarmentClothMesh(); - - // Set this mesh from a 2D garment mesh. - void Set(const b3GarmentMesh* garment); -}; - #endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h similarity index 55% rename from include/bounce/dynamics/cloth/cloth_solver.h rename to include/bounce/cloth/cloth_solver.h index ed56b82..75636f6 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -25,16 +25,9 @@ class b3StackAllocator; class b3Particle; -class b3Body; class b3Force; - -struct b3DenseVec3; -struct b3DiagMat33; -struct b3SparseSymMat33; - -class b3BodyContact; -class b3ParticleContact; -class b3TriangleContact; +class b3ParticleBodyContact; +class b3ParticleTriangleContact; struct b3ClothSolverDef { @@ -42,33 +35,9 @@ struct b3ClothSolverDef u32 particleCapacity; u32 forceCapacity; u32 bodyContactCapacity; - u32 particleContactCapacity; u32 triangleContactCapacity; }; -struct b3ClothSolverData -{ - b3DenseVec3* x; - b3DenseVec3* v; - b3DenseVec3* f; - b3DenseVec3* y; - b3SparseSymMat33* dfdx; - b3SparseSymMat33* dfdv; - b3DiagMat33* S; - b3DenseVec3* z; - float32 dt; - float32 invdt; -}; - -struct b3AccelerationConstraint -{ - u32 i1; - u32 ndof; - b3Vec3 p, q, z; - - void Apply(const b3ClothSolverData* data); -}; - class b3ClothSolver { public: @@ -77,21 +46,11 @@ public: void Add(b3Particle* p); void Add(b3Force* f); - void Add(b3BodyContact* c); - void Add(b3ParticleContact* c); - void Add(b3TriangleContact* c); + void Add(b3ParticleBodyContact* c); + void Add(b3ParticleTriangleContact* c); - void Solve(float32 dt, const b3Vec3& gravity); + void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); private: - // Apply forces. - void ApplyForces(); - - // Apply constraints. - void ApplyConstraints(); - - // Solve Ax = b. - void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; - b3StackAllocator* m_allocator; u32 m_particleCapacity; @@ -101,24 +60,14 @@ private: u32 m_forceCapacity; u32 m_forceCount; b3Force** m_forces; - - u32 m_constraintCapacity; - u32 m_constraintCount; - b3AccelerationConstraint* m_constraints; - + u32 m_bodyContactCapacity; u32 m_bodyContactCount; - b3BodyContact** m_bodyContacts; + b3ParticleBodyContact** m_bodyContacts; - u32 m_particleContactCapacity; - u32 m_particleContactCount; - b3ParticleContact** m_particleContacts; - u32 m_triangleContactCapacity; u32 m_triangleContactCount; - b3TriangleContact** m_triangleContacts; - - b3ClothSolverData m_solverData; + b3ParticleTriangleContact** m_triangleContacts; }; #endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h new file mode 100644 index 0000000..4b3f4aa --- /dev/null +++ b/include/bounce/cloth/cloth_triangle.h @@ -0,0 +1,112 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_TRIANGLE_H +#define B3_CLOTH_TRIANGLE_H + +#include + +// A cloth triangle +class b3ClothTriangle +{ +public: + // Return the triangle index. + u32 GetTriangle() const; + + // Set the triangle radius. + void SetRadius(float32 radius); + + // Return the triangle radius. + float32 GetRadius() const; + + // Set the triangle coefficient of friction. + void SetFriction(float32 friction); + + // Return the triangle coefficient of friction. + float32 GetFriction() const; +private: + friend class b3Cloth; + friend class b3Particle; + friend class b3ShearForce; + friend class b3StrechForce; + friend class b3MouseForce; + friend class b3ClothContactManager; + friend class b3ParticleTriangleContact; + friend class b3ClothSolver; + friend class b3ClothContactSolver; + + b3ClothTriangle() { } + ~b3ClothTriangle() { } + + // Synchronize AABB + void Synchronize(const b3Vec3& displacement); + + // Cloth + b3Cloth* m_cloth; + + // Triangle index + u32 m_triangle; + + // Radius + float32 m_radius; + + // Coefficient of friction + float32 m_friction; + + // AABB Proxy + b3ClothAABBProxy m_aabbProxy; + + // Broadphase ID + u32 m_broadPhaseId; + + // Alpha + float32 m_alpha; + + // Strech matrix + float32 m_du1, m_dv1; + float32 m_du2, m_dv2; + float32 m_inv_det; +}; + +inline u32 b3ClothTriangle::GetTriangle() const +{ + return m_triangle; +} + +inline void b3ClothTriangle::SetRadius(float32 radius) +{ + m_radius = radius; + Synchronize(b3Vec3_zero); +} + +inline float32 b3ClothTriangle::GetRadius() const +{ + return m_radius; +} + +inline void b3ClothTriangle::SetFriction(float32 friction) +{ + m_friction = friction; +} + +inline float32 b3ClothTriangle::GetFriction() const +{ + return m_friction; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth_contact_solver.h b/include/bounce/cloth/contacts/cloth_contact_solver.h similarity index 71% rename from include/bounce/dynamics/cloth/cloth_contact_solver.h rename to include/bounce/cloth/contacts/cloth_contact_solver.h index a4a03f5..2b33265 100644 --- a/include/bounce/dynamics/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/contacts/cloth_contact_solver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -27,11 +27,8 @@ class b3StackAllocator; class b3Particle; class b3Body; -class b3BodyContact; -class b3ParticleContact; -class b3TriangleContact; - -struct b3DenseVec3; +class b3ParticleBodyContact; +class b3ParticleTriangleContact; struct b3ClothSolverBodyContactVelocityConstraint { @@ -77,44 +74,11 @@ struct b3ClothSolverBodyContactPositionConstraint b3Vec3 rA; b3Vec3 rB; + b3Vec3 normalA; b3Vec3 localPointA; b3Vec3 localPointB; }; -struct b3ClothSolverParticleContactVelocityConstraint -{ - u32 indexA; - float32 invMassA; - - u32 indexB; - float32 invMassB; - - float32 friction; - - b3Vec3 point; - - b3Vec3 normal; - float32 normalMass; - float32 normalImpulse; - float32 velocityBias; - - b3Vec3 tangent1; - b3Vec3 tangent2; - b3Mat22 tangentMass; - b3Vec2 tangentImpulse; -}; - -struct b3ClothSolverParticleContactPositionConstraint -{ - u32 indexA; - float32 invMassA; - float32 radiusA; - - u32 indexB; - float32 invMassB; - float32 radiusB; -}; - struct b3ClothSolverTriangleContactVelocityConstraint { u32 indexA; @@ -126,14 +90,21 @@ struct b3ClothSolverTriangleContactVelocityConstraint float32 invMassC; u32 indexD; float32 invMassD; + + float32 wB, wC, wD; - b3Vec3 JA; - b3Vec3 JB; - b3Vec3 JC; - b3Vec3 JD; - + b3Vec3 normal; float32 normalMass; float32 normalImpulse; + + float32 friction; + + b3Vec3 tangent1; + b3Vec3 tangent2; + float32 tangentMass1; + float32 tangentMass2; + float32 tangentImpulse1; + float32 tangentImpulse2; }; struct b3ClothSolverTriangleContactPositionConstraint @@ -150,24 +121,21 @@ struct b3ClothSolverTriangleContactPositionConstraint float32 invMassD; float32 triangleRadius; - bool front; + float32 wB, wC, wD; }; struct b3ClothContactSolverDef { b3StackAllocator* allocator; - b3DenseVec3* positions; - b3DenseVec3* velocities; + b3Vec3* positions; + b3Vec3* velocities; u32 bodyContactCount; - b3BodyContact** bodyContacts; - - u32 particleContactCount; - b3ParticleContact** particleContacts; + b3ParticleBodyContact** bodyContacts; u32 triangleContactCount; - b3TriangleContact** triangleContacts; + b3ParticleTriangleContact** triangleContacts; }; inline float32 b3MixFriction(float32 u1, float32 u2) @@ -182,45 +150,31 @@ public: ~b3ClothContactSolver(); void InitializeBodyContactConstraints(); - - void InitializeParticleContactConstraints(); - void InitializeTriangleContactConstraints(); - void WarmStart(); + void WarmStartBodyContactConstraints(); + void WarmStartTriangleContactConstraints(); void SolveBodyContactVelocityConstraints(); - - void SolveParticleContactVelocityConstraints(); - void SolveTriangleContactVelocityConstraints(); void StoreImpulses(); bool SolveBodyContactPositionConstraints(); - - bool SolveParticleContactPositionConstraints(); - bool SolveTriangleContactPositionConstraints(); - protected: b3StackAllocator* m_allocator; - b3DenseVec3* m_positions; - b3DenseVec3* m_velocities; + b3Vec3* m_positions; + b3Vec3* m_velocities; u32 m_bodyContactCount; - b3BodyContact** m_bodyContacts; + b3ParticleBodyContact** m_bodyContacts; b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; - u32 m_particleContactCount; - b3ParticleContact** m_particleContacts; - b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints; - b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints; - u32 m_triangleContactCount; - b3TriangleContact** m_triangleContacts; + b3ParticleTriangleContact** m_triangleContacts; b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints; b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints; }; diff --git a/include/bounce/cloth/contacts/cloth_particle_body_contact.h b/include/bounce/cloth/contacts/cloth_particle_body_contact.h new file mode 100644 index 0000000..c774329 --- /dev/null +++ b/include/bounce/cloth/contacts/cloth_particle_body_contact.h @@ -0,0 +1,76 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_PARTICLE_BODY_CONTACT_H +#define B3_CLOTH_PARTICLE_BODY_CONTACT_H + +#include +#include +#include +#include + +class b3Particle; +class b3Shape; + +// A contact between a particle and a body +class b3ParticleBodyContact +{ +public: +private: + friend class b3List2; + friend class b3Cloth; + friend class b3Particle; + friend class b3ClothContactManager; + friend class b3ClothSolver; + friend class b3ClothContactSolver; + friend struct b3ParticleBodyContactWorldPoint; + + b3ParticleBodyContact() { } + ~b3ParticleBodyContact() { } + + void Update(); + + b3Particle* m_p1; + b3Shape* m_s2; + + bool m_active; + + // Contact constraint + b3Vec3 m_normal1; + b3Vec3 m_localPoint1; + b3Vec3 m_localPoint2; + float32 m_normalImpulse; + + // Friction constraint + b3Vec3 m_tangent1, m_tangent2; + b3Vec2 m_tangentImpulse; + + b3ParticleBodyContact* m_prev; + b3ParticleBodyContact* m_next; +}; + +struct b3ParticleBodyContactWorldPoint +{ + void Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + + b3Vec3 point; + b3Vec3 normal; + float32 separation; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/contacts/cloth_particle_triangle_contact.h b/include/bounce/cloth/contacts/cloth_particle_triangle_contact.h new file mode 100644 index 0000000..a5cc347 --- /dev/null +++ b/include/bounce/cloth/contacts/cloth_particle_triangle_contact.h @@ -0,0 +1,65 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_PARTICLE_TRIANGLE_CONTACT_H +#define B3_CLOTH_PARTICLE_TRIANGLE_CONTACT_H + +#include + +class b3Particle; +class b3ClothTriangle; + +// Contact between particle and a triangle +class b3ParticleTriangleContact +{ +public: +private: + friend class b3List2; + friend class b3Cloth; + friend class b3Particle; + friend class b3ClothTriangle; + friend class b3ClothContactManager; + friend class b3ClothContactSolver; + + b3ParticleTriangleContact() { } + ~b3ParticleTriangleContact() { } + + void Update(); + + // Particle + b3Particle* m_p1; + + // Triangle + b3ClothTriangle* m_t2; + b3Particle* m_p2; + b3Particle* m_p3; + b3Particle* m_p4; + + float32 m_w2, m_w3, m_w4; + + float32 m_normalImpulse; + float32 m_tangentImpulse1; + float32 m_tangentImpulse2; + + bool m_active; + + b3ParticleTriangleContact* m_prev; + b3ParticleTriangleContact* m_next; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/force.h b/include/bounce/cloth/forces/force.h similarity index 83% rename from include/bounce/dynamics/cloth/force.h rename to include/bounce/cloth/forces/force.h index 87bcb5b..27873f6 100644 --- a/include/bounce/dynamics/cloth/force.h +++ b/include/bounce/cloth/forces/force.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,16 +22,17 @@ #include #include -struct b3ClothSolverData; +struct b3ClothForceSolverData; class b3Particle; // Force types enum b3ForceType { - e_frictionForce, + e_strechForce, + e_shearForce, e_springForce, - e_bendForce, + e_mouseForce, }; struct b3ForceDef @@ -48,10 +49,13 @@ public: // b3Force* GetNext(); + + // + virtual bool HasParticle(const b3Particle* particle) const = 0; protected: friend class b3List2; friend class b3Cloth; - friend class b3ClothSolver; + friend class b3ClothForceSolver; friend class b3Particle; static b3Force* Create(const b3ForceDef* def); @@ -60,7 +64,7 @@ protected: b3Force() { } virtual ~b3Force() { } - virtual void Apply(const b3ClothSolverData* data) = 0; + virtual void Apply(const b3ClothForceSolverData* data) = 0; // Force type b3ForceType m_type; diff --git a/include/bounce/cloth/forces/mouse_force.h b/include/bounce/cloth/forces/mouse_force.h new file mode 100644 index 0000000..9646359 --- /dev/null +++ b/include/bounce/cloth/forces/mouse_force.h @@ -0,0 +1,140 @@ +/* +* 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_MOUSE_FORCE_H +#define B3_MOUSE_FORCE_H + +#include + +class b3ClothTriangle; + +struct b3MouseForceDef : public b3ForceDef +{ + b3MouseForceDef() + { + type = e_mouseForce; + } + + // Particle + b3Particle* particle; + + // Triangle + b3ClothTriangle* triangle; + + // Barycentric coordinates on triangle + float32 w2, w3, w4; + + // Mouse stiffness + float32 mouse; + + // Damping stiffness + float32 damping; +}; + +// Mouse force acting on a particle and triangle. +class b3MouseForce : public b3Force +{ +public: + bool HasParticle(const b3Particle* particle) const; + + b3Particle* GetParticle() const; + + b3ClothTriangle* GetTriangle() const; + + float32 GetMouseStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce1() const; + + b3Vec3 GetActionForce2() const; + + b3Vec3 GetActionForce3() const; + + b3Vec3 GetActionForce4() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3MouseForce(const b3MouseForceDef* def); + ~b3MouseForce(); + + void Apply(const b3ClothForceSolverData* data); + + // Solver shared + + // Particle + b3Particle* m_particle; + + // Triangle + b3ClothTriangle* m_triangle; + + // Barycentric coordinates + float32 m_w2, m_w3, m_w4; + + // Mouse stiffness + float32 m_km; + + // Damping stiffness + float32 m_kd; + + // Action forces + b3Vec3 m_f1, m_f2, m_f3, m_f4; +}; + +inline b3Particle* b3MouseForce::GetParticle() const +{ + return m_particle; +} + +inline b3ClothTriangle* b3MouseForce::GetTriangle() const +{ + return m_triangle; +} + +inline float32 b3MouseForce::GetMouseStiffness() const +{ + return m_km; +} + +inline float32 b3MouseForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3MouseForce::GetActionForce1() const +{ + return m_f1; +} + +inline b3Vec3 b3MouseForce::GetActionForce2() const +{ + return m_f2; +} + +inline b3Vec3 b3MouseForce::GetActionForce3() const +{ + return m_f3; +} + +inline b3Vec3 b3MouseForce::GetActionForce4() const +{ + return m_f4; +} + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/forces/shear_force.h b/include/bounce/cloth/forces/shear_force.h new file mode 100644 index 0000000..9092100 --- /dev/null +++ b/include/bounce/cloth/forces/shear_force.h @@ -0,0 +1,114 @@ +/* +* 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_SHEAR_FORCE_H +#define B3_SHEAR_FORCE_H + +#include + +class b3ClothTriangle; + +struct b3ShearForceDef : public b3ForceDef +{ + b3ShearForceDef() + { + type = e_shearForce; + } + + // Triangle + b3ClothTriangle* triangle; + + // Shearing stiffness + float32 shearing; + + // Damping stiffness + float32 damping; +}; + +// Shear force acting on a cloth triangle. +class b3ShearForce : public b3Force +{ +public: + bool HasParticle(const b3Particle* particle) const; + + b3ClothTriangle* GetTriangle() const; + + float32 GetShearingStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce1() const; + + b3Vec3 GetActionForce2() const; + + b3Vec3 GetActionForce3() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3ShearForce(const b3ShearForceDef* def); + ~b3ShearForce(); + + void Apply(const b3ClothForceSolverData* data); + + // Solver shared + + // Triangle + b3ClothTriangle* m_triangle; + + // Shearing stiffness + float32 m_ks; + + // Damping stiffness + float32 m_kd; + + // Action forces + b3Vec3 m_f1, m_f2, m_f3; +}; + +inline b3ClothTriangle* b3ShearForce::GetTriangle() const +{ + return m_triangle; +} + +inline float32 b3ShearForce::GetShearingStiffness() const +{ + return m_ks; +} + +inline float32 b3ShearForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3ShearForce::GetActionForce1() const +{ + return m_f1; +} + +inline b3Vec3 b3ShearForce::GetActionForce2() const +{ + return m_f2; +} + +inline b3Vec3 b3ShearForce::GetActionForce3() const +{ + return m_f3; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/spring_force.h b/include/bounce/cloth/forces/spring_force.h similarity index 87% rename from include/bounce/dynamics/cloth/spring_force.h rename to include/bounce/cloth/forces/spring_force.h index c5d1bba..3cd0a41 100644 --- a/include/bounce/dynamics/cloth/spring_force.h +++ b/include/bounce/cloth/forces/spring_force.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,7 +19,7 @@ #ifndef B3_SPRING_FORCE_H #define B3_SPRING_FORCE_H -#include +#include struct b3SpringForceDef : public b3ForceDef { @@ -67,6 +67,8 @@ public: float32 GetDampingStiffness() const; b3Vec3 GetActionForce() const; + + bool HasParticle(const b3Particle* particle) const; private: friend class b3Force; friend class b3Cloth; @@ -74,7 +76,7 @@ private: b3SpringForce(const b3SpringForceDef* def); ~b3SpringForce(); - void Apply(const b3ClothSolverData* data); + void Apply(const b3ClothForceSolverData* data); // Solver shared @@ -127,4 +129,9 @@ inline b3Vec3 b3SpringForce::GetActionForce() const return m_f; } +inline bool b3SpringForce::HasParticle(const b3Particle* particle) const +{ + return m_p1 == particle || m_p2 == particle; +} + #endif \ No newline at end of file diff --git a/include/bounce/cloth/forces/strech_force.h b/include/bounce/cloth/forces/strech_force.h new file mode 100644 index 0000000..ecbb5a2 --- /dev/null +++ b/include/bounce/cloth/forces/strech_force.h @@ -0,0 +1,126 @@ +/* +* 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_STRECH_FORCE_H +#define B3_STRECH_FORCE_H + +#include + +class b3ClothTriangle; + +struct b3StrechForceDef : public b3ForceDef +{ + b3StrechForceDef() + { + type = e_strechForce; + } + + // Triangle + b3ClothTriangle* triangle; + + // Streching stiffness + float32 streching; + + // Damping stiffness + float32 damping; + + // Desired strechiness in u direction + float32 bu; + + // Desired strechiness in v direction + float32 bv; +}; + +// Strech force acting on a cloth triangle. +class b3StrechForce : public b3Force +{ +public: + bool HasParticle(const b3Particle* particle) const; + + b3ClothTriangle* GetTriangle() const; + + float32 GetStrechingStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce1() const; + + b3Vec3 GetActionForce2() const; + + b3Vec3 GetActionForce3() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3StrechForce(const b3StrechForceDef* def); + ~b3StrechForce(); + + void Apply(const b3ClothForceSolverData* data); + + // Solver shared + + // Triangle + b3ClothTriangle* m_triangle; + + // Streching stiffness + float32 m_ks; + + // Damping stiffness + float32 m_kd; + + // Desired strechiness in u direction + float32 m_bu; + + // Desired strechiness in v direction + float32 m_bv; + + // Action forces + b3Vec3 m_f1, m_f2, m_f3; +}; + +inline b3ClothTriangle* b3StrechForce::GetTriangle() const +{ + return m_triangle; +} + +inline float32 b3StrechForce::GetStrechingStiffness() const +{ + return m_ks; +} + +inline float32 b3StrechForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3StrechForce::GetActionForce1() const +{ + return m_f1; +} + +inline b3Vec3 b3StrechForce::GetActionForce2() const +{ + return m_f2; +} + +inline b3Vec3 b3StrechForce::GetActionForce3() const +{ + return m_f3; +} + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/garment/garment.h b/include/bounce/cloth/garment/garment.h new file mode 100644 index 0000000..202abe2 --- /dev/null +++ b/include/bounce/cloth/garment/garment.h @@ -0,0 +1,72 @@ +/* +* 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_GARMENT_H +#define B3_GARMENT_H + +#include + +struct b3SewingLine +{ + u32 p1, p2; + u32 v1, v2; +}; + +struct b3Garment +{ + u32 patternCount; + b3SewingPattern** patterns; + u32 sewingCount; + b3SewingLine* sewingLines; +}; + +struct b3RectangleGarment : public b3Garment +{ + b3RectanglePattern rectangle; + b3SewingPattern* rectanglePatterns[1]; + + b3RectangleGarment(float32 ex = 1.0f, float32 ey = 1.0f) : rectangle(ex, ey) + { + rectanglePatterns[0] = &rectangle; + + patternCount = 1; + patterns = rectanglePatterns; + + sewingCount = 0; + sewingLines = nullptr; + } +}; + +struct b3CircleGarment : public b3Garment +{ + b3CirclePattern<> circle; + b3SewingPattern* circlePatterns[1]; + + b3CircleGarment(float32 r = 1.0f) : circle(r) + { + circlePatterns[0] = &circle; + + patternCount = 1; + patterns = circlePatterns; + + sewingCount = 0; + sewingLines = nullptr; + } +}; + +#endif \ No newline at end of file diff --git a/include/bounce/garment/garment_mesh.h b/include/bounce/cloth/garment/garment_mesh.h similarity index 95% rename from include/bounce/garment/garment_mesh.h rename to include/bounce/cloth/garment/garment_mesh.h index 66e97ca..9057d24 100644 --- a/include/bounce/garment/garment_mesh.h +++ b/include/bounce/cloth/garment/garment_mesh.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/garment/sewing_pattern.h b/include/bounce/cloth/garment/sewing_pattern.h similarity index 96% rename from include/bounce/garment/sewing_pattern.h rename to include/bounce/cloth/garment/sewing_pattern.h index 3d2ee57..635bed9 100644 --- a/include/bounce/garment/sewing_pattern.h +++ b/include/bounce/cloth/garment/sewing_pattern.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/cloth/garment_cloth_mesh.h b/include/bounce/cloth/garment_cloth_mesh.h new file mode 100644 index 0000000..c608077 --- /dev/null +++ b/include/bounce/cloth/garment_cloth_mesh.h @@ -0,0 +1,37 @@ +/* +* 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_GARMENT_CLOTH_MESH_H +#define B3_GARMENT_CLOTH_MESH_H + +#include + +struct b3GarmentMesh; + +// This mesh structure represents a cloth mesh that can be set from 2D a garment mesh. +// The mesh is set in the xy plane (z = 0). +struct b3GarmentClothMesh : public b3ClothMesh +{ + b3GarmentClothMesh(); + ~b3GarmentClothMesh(); + + // Set this mesh from a 2D garment mesh. + void Set(const b3GarmentMesh* garment); +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/grid_cloth_mesh.h b/include/bounce/cloth/grid_cloth_mesh.h new file mode 100644 index 0000000..474daeb --- /dev/null +++ b/include/bounce/cloth/grid_cloth_mesh.h @@ -0,0 +1,96 @@ +/* +* 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_GRID_CLOTH_MESH_H +#define B3_GRID_CLOTH_MESH_H + +#include + +// A (H + 1) x (W + 1) grid mesh stored in row-major order. +// v(i, j) = i + (W + 1) + j +template +struct b3GridClothMesh : public b3ClothMesh +{ + b3Vec3 gridVertices[(H + 1) * (W + 1)]; + b3ClothMeshTriangle gridTriangles[2 * H * W]; + b3ClothMeshMesh gridMesh; + + // Set this grid to a W (width) per H (height) dimensioned grid centered at the origin and aligned + // with the world x-z axes. + b3GridClothMesh() + { + vertexCount = 0; + for (u32 i = 0; i <= H; ++i) + { + for (u32 j = 0; j <= W; ++j) + { + gridVertices[vertexCount++].Set(float32(j), 0.0f, float32(i)); + } + } + + B3_ASSERT(vertexCount == (W + 1) * (H + 1)); + + b3Vec3 translation; + translation.x = -0.5f * float32(W); + translation.y = 0.0f; + translation.z = -0.5f * float32(H); + + for (u32 i = 0; i < vertexCount; ++i) + { + gridVertices[i] += translation; + } + + triangleCount = 0; + for (u32 i = 0; i < H; ++i) + { + for (u32 j = 0; j < W; ++j) + { + u32 v1 = i * (W + 1) + j; + u32 v2 = (i + 1) * (W + 1) + j; + u32 v3 = (i + 1) * (W + 1) + (j + 1); + u32 v4 = i * (W + 1) + (j + 1); + + b3ClothMeshTriangle* t1 = gridTriangles + triangleCount++; + t1->v1 = v3; + t1->v2 = v2; + t1->v3 = v1; + + b3ClothMeshTriangle* t2 = gridTriangles + triangleCount++; + t2->v1 = v1; + t2->v2 = v4; + t2->v3 = v3; + } + } + + B3_ASSERT(triangleCount == 2 * H * W); + + gridMesh.startTriangle = 0; + gridMesh.triangleCount = triangleCount; + gridMesh.startVertex = 0; + gridMesh.vertexCount = vertexCount; + + vertices = gridVertices; + triangles = gridTriangles; + meshCount = 1; + meshes = &gridMesh; + sewingLineCount = 0; + sewingLines = nullptr; + } +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/cloth/particle.h similarity index 73% rename from include/bounce/dynamics/cloth/particle.h rename to include/bounce/cloth/particle.h index 11b93d0..a4d9aef 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,14 +19,10 @@ #ifndef B3_PARTICLE_H #define B3_PARTICLE_H -#include +#include #include -#include -#include -class b3Shape; class b3Cloth; -class b3Particle; // Static particle: Can be moved manually. // Kinematic particle: Non-zero velocity, can be moved by the solver. @@ -38,12 +34,13 @@ enum b3ParticleType e_dynamicParticle }; -// +// Particle definition struct b3ParticleDef { b3ParticleDef() { type = e_staticParticle; + mass = 0.0f; position.SetZero(); velocity.SetZero(); force.SetZero(); @@ -53,6 +50,7 @@ struct b3ParticleDef } b3ParticleType type; + float32 mass; b3Vec3 position; b3Vec3 velocity; b3Vec3 force; @@ -61,89 +59,6 @@ struct b3ParticleDef void* userData; }; -// A contact between a particle and a solid -class b3BodyContact -{ -public: - b3BodyContact() { } - ~b3BodyContact() { } - - b3Particle* p1; - b3Shape* s2; - - // Contact constraint - b3Vec3 localPoint1; - b3Vec3 localPoint2; - float32 normalImpulse; - - // Friction constraint - b3Vec3 t1, t2; - b3Vec2 tangentImpulse; - - b3BodyContact* m_prev; - b3BodyContact* m_next; -}; - -struct b3BodyContactWorldPoint -{ - void Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; - -// A contact between two particles -class b3ParticleContact -{ -public: - b3ParticleContact() { } - ~b3ParticleContact() { } - - b3Particle* p1; - b3Particle* p2; - - // Contact constraint - float32 normalImpulse; - - // Friction constraint - b3Vec3 t1, t2; - b3Vec2 tangentImpulse; - - b3ParticleContact* m_prev; - b3ParticleContact* m_next; -}; - -struct b3ParticleContactWorldPoint -{ - void Initialize(const b3ParticleContact* c); - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; - -// A contact between a particle and a triangle -class b3TriangleContact -{ -public: - b3TriangleContact() { } - ~b3TriangleContact() { } - - b3Particle* p1; - b3Particle* p2; - b3Particle* p3; - b3Particle* p4; - - bool front; - - // Contact constraint - float32 normalImpulse; - - b3TriangleContact* m_prev; - b3TriangleContact* m_next; -}; - // A cloth particle. class b3Particle { @@ -197,16 +112,31 @@ public: private: friend class b3List2; friend class b3Cloth; + friend class b3ClothContactManager; friend class b3ClothSolver; + friend class b3ClothForceSolver; + friend class b3ClothTriangle; + friend class b3ParticleBodyContact; + friend class b3ParticleTriangleContact; friend class b3ClothContactSolver; friend class b3Force; + friend class b3StrechForce; + friend class b3ShearForce; friend class b3SpringForce; - friend class b3BendForce; - friend class b3FrictionForce; + friend class b3MouseForce; b3Particle(const b3ParticleDef& def, b3Cloth* cloth); ~b3Particle(); + // Synchronize particle AABB + void Synchronize(const b3Vec3& displacement); + + // Synchronize triangles AABB + void SynchronizeTriangles(); + + // Destroy contacts. + void DestroyContacts(); + // Type b3ParticleType m_type; @@ -219,7 +149,7 @@ private: // Applied external force b3Vec3 m_force; - // Applied external translation + // Applied translation b3Vec3 m_translation; // Mass @@ -248,13 +178,17 @@ private: // Solution b3Vec3 m_x; - // + // Parent cloth b3Cloth* m_cloth; - // - b3Particle* m_prev; + // AABB Proxy + b3ClothAABBProxy m_aabbProxy; - // + // Broadphase ID + u32 m_broadPhaseId; + + // Links to the cloth particle list. + b3Particle* m_prev; b3Particle* m_next; }; @@ -270,8 +204,13 @@ inline u32 b3Particle::GetVertex() const inline void b3Particle::SetPosition(const b3Vec3& position) { + b3Vec3 displacement = position - m_position; + m_position = position; m_translation.SetZero(); + + Synchronize(displacement); + SynchronizeTriangles(); } inline const b3Vec3& b3Particle::GetPosition() const @@ -301,6 +240,8 @@ inline float32 b3Particle::GetMass() const inline void b3Particle::SetRadius(float32 radius) { m_radius = radius; + + Synchronize(b3Vec3_zero); } inline float32 b3Particle::GetRadius() const diff --git a/include/bounce/collision/broad_phase.h b/include/bounce/collision/broad_phase.h index 8e703f6..9c37a6e 100644 --- a/include/bounce/collision/broad_phase.h +++ b/include/bounce/collision/broad_phase.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,6 +22,8 @@ #include #include +#define B3_NULL_PROXY (0xFFFFFFFF) + // A pair of broad-phase proxies. struct b3Pair { @@ -49,9 +51,8 @@ public: // Return true if the proxy has moved. bool MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement); - // Add a proxy to the list of moved proxies. - // Only moved proxies will be used internally as an AABB query reference object. - void BufferMove(u32 proxyId); + // Force move the proxy + void TouchProxy(u32 proxyId); // Get the AABB of a given proxy. const b3AABB3& GetAABB(u32 proxyId) const; @@ -59,6 +60,9 @@ public: // Get the user data attached to a proxy. void* GetUserData(u32 proxyId) const; + // Get the number of proxies. + u32 GetProxyCount() const; + // Test if two proxy AABBs are overlapping. bool TestOverlap(u32 proxy1, u32 proxy2) const; @@ -81,6 +85,9 @@ public: void Draw() const; private : friend class b3DynamicTree; + + void BufferMove(u32 proxyId); + void UnbufferMove(u32 proxyId); // The client callback used to add an overlapping pair // to the overlapping pair buffer. @@ -89,6 +96,9 @@ private : // The dynamic tree. b3DynamicTree m_tree; + // Number of proxies + u32 m_proxyCount; + // The current proxy being queried for overlap with another proxies. // It is used to avoid a proxy overlap with itself. u32 m_queryProxyId; @@ -114,6 +124,11 @@ inline void* b3BroadPhase::GetUserData(u32 proxyId) const return m_tree.GetUserData(proxyId); } +inline u32 b3BroadPhase::GetProxyCount() const +{ + return m_proxyCount; +} + template inline void b3BroadPhase::QueryAABB(T* callback, const b3AABB3& aabb) const { @@ -152,8 +167,10 @@ inline void b3BroadPhase::FindPairs(T* callback) { // Keep the current queried proxy ID to avoid self overlapping. m_queryProxyId = m_moveBuffer[i]; - if (m_queryProxyId == B3_NULL_NODE_D) + + if (m_queryProxyId == B3_NULL_PROXY) { + // Proxy was unbuffered continue; } diff --git a/include/bounce/collision/collision.h b/include/bounce/collision/collision.h index 1fde28f..4201dc3 100644 --- a/include/bounce/collision/collision.h +++ b/include/bounce/collision/collision.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -37,4 +37,8 @@ struct b3RayCastOutput b3Vec3 normal; // surface normal of intersection }; +// Perform a ray-cast on a triangle. +bool b3RayCast(b3RayCastOutput* output, const b3RayCastInput* input, + const b3Vec3& v1, const b3Vec3& v2, const b3Vec3& v3); + #endif \ No newline at end of file diff --git a/include/bounce/collision/gjk/gjk.h b/include/bounce/collision/gjk/gjk.h index 2fd5459..0d85e50 100644 --- a/include/bounce/collision/gjk/gjk.h +++ b/include/bounce/collision/gjk/gjk.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -114,4 +114,18 @@ b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1, const b3Transform& xf2, const b3GJKProxy& proxy2, bool applyRadius, b3SimplexCache* cache); +// The output of the GJK-based shape cast algorithm. +struct b3GJKShapeCastOutput +{ + float32 t; // time of impact + b3Vec3 point; // contact point at t + b3Vec3 normal; // contact normal at t + u32 iterations; // number of iterations +}; + +// Find the time of impact between two proxies given the relative target translation vector. +bool b3GJKShapeCast(b3GJKShapeCastOutput* output, + const b3Transform& xf1, const b3GJKProxy& proxy1, + const b3Transform& xf2, const b3GJKProxy& proxy2, const b3Vec3& translation2); + #endif \ No newline at end of file diff --git a/include/bounce/collision/gjk/gjk_proxy.h b/include/bounce/collision/gjk/gjk_proxy.h index 5a4766e..71638ea 100644 --- a/include/bounce/collision/gjk/gjk_proxy.h +++ b/include/bounce/collision/gjk/gjk_proxy.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/sat/sat.h b/include/bounce/collision/sat/sat.h index 8f45ee2..a21fa12 100644 --- a/include/bounce/collision/sat/sat.h +++ b/include/bounce/collision/sat/sat.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/sat/sat_edge_and_hull.h b/include/bounce/collision/sat/sat_edge_and_hull.h index 60ecb2f..8998a40 100644 --- a/include/bounce/collision/sat/sat_edge_and_hull.h +++ b/include/bounce/collision/sat/sat_edge_and_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/sat/sat_vertex_and_hull.h b/include/bounce/collision/sat/sat_vertex_and_hull.h index 52a59d5..4f78c32 100644 --- a/include/bounce/collision/sat/sat_vertex_and_hull.h +++ b/include/bounce/collision/sat/sat_vertex_and_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/shapes/aabb3.h b/include/bounce/collision/shapes/aabb3.h index 46f9c5a..51ba36e 100644 --- a/include/bounce/collision/shapes/aabb3.h +++ b/include/bounce/collision/shapes/aabb3.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -37,8 +37,8 @@ struct b3AABB3 return support; } - // Compute this AABB from a list of points. - void Compute(const b3Vec3* points, u32 count) + // Set this AABB from a list of points. + void Set(const b3Vec3* points, u32 count) { m_lower = m_upper = points[0]; for (u32 i = 1; i < count; ++i) @@ -48,8 +48,8 @@ struct b3AABB3 } } - // Compute this AABB from a list of points and a transform. - void Compute(const b3Vec3* points, u32 count, const b3Transform& xf) + // Set this AABB from a list of points and a transform. + void Set(const b3Vec3* points, u32 count, const b3Transform& xf) { m_lower = m_upper = b3Mul(xf, points[0]); for (u32 i = 1; i < count; ++i) @@ -60,6 +60,27 @@ struct b3AABB3 } } + // Set this AABB from a triangle. + void Set(const b3Vec3& v1, const b3Vec3& v2, const b3Vec3& v3) + { + m_lower = b3Min(v1, b3Min(v2, v3)); + m_upper = b3Max(v1, b3Max(v2, v3)); + } + + // Set this AABB from a center point and a radius vector. + void Set(const b3Vec3& center, const b3Vec3& r) + { + m_lower = center - r; + m_upper = center + r; + } + + // Set this AABB from a center point and a radius value. + void Set(const b3Vec3& center, float32 radius) + { + b3Vec3 r(radius, radius, radius); + Set(center, r); + } + // Extend this AABB by a scalar. void Extend(float32 s) { @@ -146,40 +167,70 @@ struct b3AABB3 } // Test if a ray intersects this AABB. - // Output the minimum and maximum intersection fractions to derive the minimum and maximum intersection points. - bool TestRay(const b3Vec3& p1, const b3Vec3& p2, float32& min_t, float32& max_t) const + bool TestRay(float32& minFraction, const b3Vec3& p1, const b3Vec3& p2, float32 maxFraction) const { b3Vec3 d = p2 - p1; - float32 t = d.Normalize(); - B3_ASSERT(t > B3_EPSILON); - - b3Vec3 inv_d; - inv_d.x = 1.0f / d.x; - inv_d.y = 1.0f / d.y; - inv_d.z = 1.0f / d.z; - - b3Vec3 t1; - t1.x = (m_lower.x - p1.x) * inv_d.x; - t1.y = (m_lower.y - p1.y) * inv_d.y; - t1.z = (m_lower.z - p1.z) * inv_d.z; - - b3Vec3 t2; - t2.x = (m_upper.x - p1.x) * inv_d.x; - t2.y = (m_upper.y - p1.y) * inv_d.y; - t2.z = (m_upper.z - p1.z) * inv_d.z; - + + float32 lower = 0.0f; + float32 upper = maxFraction; + for (u32 i = 0; i < 3; ++i) { - min_t = b3Max(min_t, b3Min(t1[i], t2[i])); - max_t = b3Min(max_t, b3Max(t1[i], t2[i])); + float32 numerators[2], denominators[2]; + + numerators[0] = p1[i] - m_lower[i]; + numerators[1] = m_upper[i] - p1[i]; + + denominators[0] = -d[i]; + denominators[1] = d[i]; + + for (u32 j = 0; j < 2; ++j) + { + float32 numerator = numerators[j]; + float32 denominator = denominators[j]; + + if (denominator == 0.0f) + { + // s is parallel to this half-space. + if (numerator < 0.0f) + { + // s is outside of this half-space. + // dot(n, p1) and dot(n, p2) < 0. + return false; + } + } + else + { + if (denominator < 0.0f) + { + // s enters this half-space. + if (numerator < lower * denominator) + { + // Increase lower. + lower = numerator / denominator; + } + } + else + { + // s exits the half-space. + if (numerator < upper * denominator) + { + // Decrease upper. + upper = numerator / denominator; + } + } + // Exit if intersection becomes empty. + if (upper < lower) + { + return false; + } + } + } } - if (min_t >= 0.0f && min_t >= max_t && max_t <= t) - { - return true; - } - - return false; + B3_ASSERT(lower >= 0.0f && lower <= maxFraction); + minFraction = lower; + return true; } }; diff --git a/include/bounce/collision/shapes/box_hull.h b/include/bounce/collision/shapes/box_hull.h index 5d384fa..b655c18 100644 --- a/include/bounce/collision/shapes/box_hull.h +++ b/include/bounce/collision/shapes/box_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/shapes/capsule.h b/include/bounce/collision/shapes/capsule.h index 3c5464b..ea434e5 100644 --- a/include/bounce/collision/shapes/capsule.h +++ b/include/bounce/collision/shapes/capsule.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/shapes/grid_mesh.h b/include/bounce/collision/shapes/grid_mesh.h index 4561277..f334569 100644 --- a/include/bounce/collision/shapes/grid_mesh.h +++ b/include/bounce/collision/shapes/grid_mesh.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,6 +22,7 @@ #include // A (H + 1) x (W + 1) grid mesh stored in row-major order. +// v(i, j) = i * (W + 1) + j template struct b3GridMesh : public b3Mesh { @@ -32,61 +33,53 @@ struct b3GridMesh : public b3Mesh // with the world x-z axes. b3GridMesh() { - u32 h = H + 1; - u32 w = W + 1; - - b3Vec3 t; - t.x = -0.5f * float32(w) + 0.5f; - t.y = 0.0f; - t.z = -0.5f * float32(h) + 0.5f; - - for (u32 i = 0; i < h; ++i) + vertexCount = 0; + for (u32 i = 0; i <= H; ++i) { - for (u32 j = 0; j < w; ++j) + for (u32 j = 0; j <= W; ++j) { - u32 v1 = i * w + j; - - b3Vec3 v; - v.x = float32(j); - v.y = 0.0f; - v.z = float32(i); - - v += t; - - gridVertices[v1] = v; + gridVertices[vertexCount++].Set(float32(j), 0.0f, float32(i)); } } - u32 triangleIndex = 0; - for (u32 i = 0; i < h - 1; ++i) + B3_ASSERT(vertexCount == (H + 1) * (W + 1)); + + b3Vec3 translation; + translation.x = -0.5f * float32(W); + translation.y = 0.0f; + translation.z = -0.5f * float32(H); + + for (u32 i = 0; i < vertexCount; ++i) { - for (u32 j = 0; j < w - 1; ++j) + gridVertices[i] += translation; + } + + triangleCount = 0; + for (u32 i = 0; i < H; ++i) + { + for (u32 j = 0; j < W; ++j) { - u32 v1 = i * w + j; - u32 v2 = (i + 1) * w + j; - u32 v3 = (i + 1) * w + (j + 1); - u32 v4 = i * w + (j + 1); - - b3Triangle* t1 = gridTriangles + triangleIndex; - ++triangleIndex; + u32 v1 = i * (W + 1) + j; + u32 v2 = (i + 1) * (W + 1) + j; + u32 v3 = (i + 1) * (W + 1) + (j + 1); + u32 v4 = i * (W + 1) + (j + 1); + b3Triangle* t1 = gridTriangles + triangleCount++; t1->v1 = v3; t1->v2 = v2; t1->v3 = v1; - b3Triangle* t2 = gridTriangles + triangleIndex; - ++triangleIndex; - + b3Triangle* t2 = gridTriangles + triangleCount++; t2->v1 = v1; t2->v2 = v4; t2->v3 = v3; } } + B3_ASSERT(triangleCount == 2 * H * W); + vertices = gridVertices; - vertexCount = (H + 1) * (W + 1); triangles = gridTriangles; - triangleCount = 2 * H * W; } }; diff --git a/include/bounce/collision/shapes/hull.h b/include/bounce/collision/shapes/hull.h index 46a7f11..48f152d 100644 --- a/include/bounce/collision/shapes/hull.h +++ b/include/bounce/collision/shapes/hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/shapes/mesh.h b/include/bounce/collision/shapes/mesh.h index a5395dc..60586b1 100644 --- a/include/bounce/collision/shapes/mesh.h +++ b/include/bounce/collision/shapes/mesh.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -78,14 +78,10 @@ inline u32 b3Mesh::GetSize() const inline b3AABB3 b3Mesh::GetTriangleAABB(u32 index) const { const b3Triangle* triangle = triangles + index; - - u32 i1 = triangle->v1; - u32 i2 = triangle->v2; - u32 i3 = triangle->v3; b3AABB3 aabb; - aabb.m_lower = b3Min(b3Min(vertices[i1], vertices[i2]), vertices[i3]); - aabb.m_upper = b3Max(b3Max(vertices[i1], vertices[i2]), vertices[i3]); + aabb.Set(vertices[triangle->v1], vertices[triangle->v2], vertices[triangle->v3]); + return aabb; } diff --git a/include/bounce/collision/shapes/qhull.h b/include/bounce/collision/shapes/qhull.h index c900135..25836de 100644 --- a/include/bounce/collision/shapes/qhull.h +++ b/include/bounce/collision/shapes/qhull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -32,13 +32,13 @@ struct b3QHull : public b3Hull b3QHull() { - vertices = hullVertices.Begin(); + vertices = nullptr; vertexCount = 0; - edges = hullEdges.Begin(); + edges = nullptr; edgeCount = 0; - faces = hullFaces.Begin(); + faces = nullptr; faceCount = 0; - planes = hullPlanes.Begin(); + planes = nullptr; centroid.SetZero(); } diff --git a/include/bounce/collision/shapes/sphere.h b/include/bounce/collision/shapes/sphere.h index 7156fb0..b0bc4bb 100644 --- a/include/bounce/collision/shapes/sphere.h +++ b/include/bounce/collision/shapes/sphere.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/shapes/triangle_hull.h b/include/bounce/collision/shapes/triangle_hull.h index 1ed5964..92c67ef 100644 --- a/include/bounce/collision/shapes/triangle_hull.h +++ b/include/bounce/collision/shapes/triangle_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/collision/trees/dynamic_tree.h b/include/bounce/collision/trees/dynamic_tree.h index 227d68b..969f46d 100644 --- a/include/bounce/collision/trees/dynamic_tree.h +++ b/include/bounce/collision/trees/dynamic_tree.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -211,8 +211,8 @@ inline void b3DynamicTree::RayCast(T* callback, const b3RayCastInput& input) con const b3Node* node = m_nodes + nodeIndex; - float32 minFraction = 0.0f; - if (node->aabb.TestRay(p1, p2, maxFraction, minFraction) == true) + float32 minFraction; + if (node->aabb.TestRay(minFraction, p1, p2, maxFraction) == true) { if (node->IsLeaf() == true) { diff --git a/include/bounce/collision/trees/static_tree.h b/include/bounce/collision/trees/static_tree.h index 1af50ad..4a07ba1 100644 --- a/include/bounce/collision/trees/static_tree.h +++ b/include/bounce/collision/trees/static_tree.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -174,8 +174,8 @@ inline void b3StaticTree::RayCast(T* callback, const b3RayCastInput& input) cons const b3Node* node = m_nodes + nodeIndex; - float32 minFraction = 0.0f; - if (node->aabb.TestRay(p1, p2, maxFraction, minFraction) == true) + float32 minFraction; + if (node->aabb.TestRay(minFraction, p1, p2, maxFraction) == true) { if (node->IsLeaf() == true) { diff --git a/include/bounce/common/draw.h b/include/bounce/common/draw.h index cbb2b51..bc5f1d9 100644 --- a/include/bounce/common/draw.h +++ b/include/bounce/common/draw.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -95,17 +95,23 @@ public : // Draw a solid circle with center, normal, and radius. virtual void DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0; - // Draw a sphere with center and radius. + // Draw a plane with center, normal and radius. + virtual void DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0; + + // Draw a solid plane with center, normal and radius. + virtual void DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0; + + // Draw a sphere with center, and radius. virtual void DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0; - // Draw a solid sphere with center and radius. - virtual void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0; + // Draw a solid sphere with center, radius, and rotation. + virtual void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Mat33& rotation, const b3Color& color) = 0; - // Draw a capsule with segment and radius. + // Draw a capsule with segment, and radius. virtual void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0; - // Draw a solid capsule with segment and radius. - virtual void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0; + // Draw a solid capsule with segment, radius, and rotation. + virtual void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Mat33& rotation, const b3Color& color) = 0; // Draw a AABB. virtual void DrawAABB(const b3AABB3& aabb, const b3Color& color) = 0; diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h index 7cb1582..50adc14 100644 --- a/include/bounce/common/geometry.h +++ b/include/bounce/common/geometry.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -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) { diff --git a/include/bounce/common/math/mat.h b/include/bounce/common/math/mat.h index 75eab32..0f9cd93 100644 --- a/include/bounce/common/math/mat.h +++ b/include/bounce/common/math/mat.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/mat22.h b/include/bounce/common/math/mat22.h index bc66b82..b976cf1 100644 --- a/include/bounce/common/math/mat22.h +++ b/include/bounce/common/math/mat22.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/mat33.h b/include/bounce/common/math/mat33.h index 3696e22..89da221 100644 --- a/include/bounce/common/math/mat33.h +++ b/include/bounce/common/math/mat33.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -48,6 +48,12 @@ struct b3Mat33 return (&x.x)[i + 3 * j]; } + // Write an indexed element from this matrix. + float32& operator()(u32 i, u32 j) + { + return (&x.x)[i + 3 * j]; + } + // Add a matrix to this matrix. void operator+=(const b3Mat33& B) { @@ -227,22 +233,20 @@ inline b3Mat33 b3Outer(const b3Vec3& a, const b3Vec3& b) // Compute an orthogonal basis given one of its vectors. // The vector must be normalized. -inline b3Mat33 b3Basis(const b3Vec3& a) +inline void b3ComputeBasis(const b3Vec3& a, b3Vec3& b, b3Vec3& c) { - // Box2D - b3Mat33 A; + // https://box2d.org/2014/02/computing-a-basis/ if (b3Abs(a.x) >= float32(0.57735027)) { - A.y.Set(a.y, -a.x, 0.0f); + b.Set(a.y, -a.x, 0.0f); } else { - A.y.Set(0.0f, a.z, -a.y); + b.Set(0.0f, a.z, -a.y); } - A.x = a; - A.y = b3Normalize(A.y); - A.z = b3Cross(a, A.y); - return A; + + b.Normalize(); + c = b3Cross(a, b); } // Rotation about the x-axis. diff --git a/include/bounce/common/math/mat44.h b/include/bounce/common/math/mat44.h index 0285422..57691e2 100644 --- a/include/bounce/common/math/mat44.h +++ b/include/bounce/common/math/mat44.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/math.h b/include/bounce/common/math/math.h index 4bbc091..3521d30 100644 --- a/include/bounce/common/math/math.h +++ b/include/bounce/common/math/math.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h index 7f745ac..a027a99 100644 --- a/include/bounce/common/math/quat.h +++ b/include/bounce/common/math/quat.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/transform.h b/include/bounce/common/math/transform.h index 13a9101..da6530e 100644 --- a/include/bounce/common/math/transform.h +++ b/include/bounce/common/math/transform.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/vec2.h b/include/bounce/common/math/vec2.h index 4fcaac9..b833016 100644 --- a/include/bounce/common/math/vec2.h +++ b/include/bounce/common/math/vec2.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/vec3.h b/include/bounce/common/math/vec3.h index 0b04bda..67c2524 100644 --- a/include/bounce/common/math/vec3.h +++ b/include/bounce/common/math/vec3.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/math/vec4.h b/include/bounce/common/math/vec4.h index 304e45f..8b44ec8 100644 --- a/include/bounce/common/math/vec4.h +++ b/include/bounce/common/math/vec4.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/memory/block_pool.h b/include/bounce/common/memory/block_pool.h index 583b6b6..3f81955 100644 --- a/include/bounce/common/memory/block_pool.h +++ b/include/bounce/common/memory/block_pool.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/memory/stack_allocator.h b/include/bounce/common/memory/stack_allocator.h index db64bb8..be70249 100644 --- a/include/bounce/common/memory/stack_allocator.h +++ b/include/bounce/common/memory/stack_allocator.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/settings.h b/include/bounce/common/settings.h index 7aeba36..431ab59 100644 --- a/include/bounce/common/settings.h +++ b/include/bounce/common/settings.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/template/array.h b/include/bounce/common/template/array.h index e920edd..49b4069 100644 --- a/include/bounce/common/template/array.h +++ b/include/bounce/common/template/array.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/template/list.h b/include/bounce/common/template/list.h index 58c00b6..8d83141 100644 --- a/include/bounce/common/template/list.h +++ b/include/bounce/common/template/list.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/template/object_array.h b/include/bounce/common/template/object_array.h index 8f4a885..348f9e2 100644 --- a/include/bounce/common/template/object_array.h +++ b/include/bounce/common/template/object_array.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -93,6 +93,9 @@ public: { B3_ASSERT(m_count > 0); --m_count; + + T* e = m_elements + m_count; + e->~T(); } const T& Back() const diff --git a/include/bounce/common/template/queue.h b/include/bounce/common/template/queue.h index 327f169..0992b4d 100644 --- a/include/bounce/common/template/queue.h +++ b/include/bounce/common/template/queue.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/template/stack.h b/include/bounce/common/template/stack.h index 46d28bd..ca938d0 100644 --- a/include/bounce/common/template/stack.h +++ b/include/bounce/common/template/stack.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/common/time.h b/include/bounce/common/time.h index 5db5a63..f7bf0d6 100644 --- a/include/bounce/common/time.h +++ b/include/bounce/common/time.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/controllers/body_dragger.h b/include/bounce/controllers/body_dragger.h deleted file mode 100644 index 37b28dd..0000000 --- a/include/bounce/controllers/body_dragger.h +++ /dev/null @@ -1,129 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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_BODY_DRAGGER_H -#define B3_BODY_DRAGGER_H - -#include -#include - -// A body dragger. -class b3BodyDragger -{ -public: - b3BodyDragger(b3Ray3* ray, b3World* world) - { - m_ray = ray; - m_world = world; - m_shape = nullptr; - m_mouseJoint = nullptr; - } - - ~b3BodyDragger() - { - - } - - bool IsDragging() const - { - return m_shape != nullptr; - } - - bool StartDragging() - { - B3_ASSERT(IsDragging() == false); - - b3RayCastSingleShapeOutput out; - if (m_world->RayCastSingleShape(&out, m_ray->A(), m_ray->B()) == false) - { - return false; - } - - m_x = out.fraction; - m_shape = out.shape; - - b3BodyDef bd; - b3Body* groundBody = m_world->CreateBody(bd); - - b3Body* body = m_shape->GetBody(); - body->SetAwake(true); - - b3MouseJointDef jd; - jd.bodyA = groundBody; - jd.bodyB = body; - jd.target = out.point; - jd.maxForce = 2000.0f * body->GetMass(); - - m_mouseJoint = (b3MouseJoint*)m_world->CreateJoint(jd); - - m_p = body->GetLocalPoint(out.point); - - return true; - } - - void Drag() - { - B3_ASSERT(IsDragging() == true); - m_mouseJoint->SetTarget(GetPointB()); - } - - void StopDragging() - { - B3_ASSERT(IsDragging() == true); - b3Body* groundBody = m_mouseJoint->GetBodyA(); - m_world->DestroyJoint(m_mouseJoint); - m_mouseJoint = nullptr; - m_world->DestroyBody(groundBody); - m_shape = nullptr; - } - - b3Ray3* GetRay() const - { - return m_ray; - } - - b3Body* GetBody() const - { - B3_ASSERT(IsDragging() == true); - return m_shape->GetBody(); - } - - b3Vec3 GetPointA() const - { - B3_ASSERT(IsDragging() == true); - return m_shape->GetBody()->GetWorldPoint(m_p); - } - - b3Vec3 GetPointB() const - { - B3_ASSERT(IsDragging() == true); - return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); - } - -private: - b3Ray3 * m_ray; - float32 m_x; - - b3World* m_world; - - b3Shape* m_shape; - b3Vec3 m_p; - b3MouseJoint* m_mouseJoint; -}; - -#endif \ No newline at end of file diff --git a/include/bounce/controllers/cloth_dragger.h b/include/bounce/controllers/cloth_dragger.h deleted file mode 100644 index b77060a..0000000 --- a/include/bounce/controllers/cloth_dragger.h +++ /dev/null @@ -1,224 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B3_CLOTH_DRAGGER_H -#define B3_CLOTH_DRAGGER_H - -#include -#include -#include -#include -#include -#include - -// A cloth triangle dragger. -class b3ClothDragger -{ -public: - b3ClothDragger(b3Ray3* ray, b3World* world) - { - m_spring = false; - m_ray = ray; - m_world = world; - m_cloth = nullptr; - } - - ~b3ClothDragger() - { - - } - - bool IsDragging() const - { - return m_cloth != nullptr; - } - - bool StartDragging() - { - B3_ASSERT(IsDragging() == false); - - b3RayCastSingleClothOutput rayOut; - if (m_world->RayCastSingleCloth(&rayOut, m_ray->A(), m_ray->B()) == false) - { - return false; - } - - m_cloth = rayOut.cloth; - m_mesh = m_cloth->GetMesh(); - m_triangle = m_mesh->triangles + rayOut.triangle; - m_x = rayOut.fraction; - - b3Particle* p1 = m_cloth->GetVertexParticle(m_triangle->v1); - b3Particle* p2 = m_cloth->GetVertexParticle(m_triangle->v2); - b3Particle* p3 = m_cloth->GetVertexParticle(m_triangle->v3); - - b3Vec3 v1 = p1->GetPosition(); - b3Vec3 v2 = p2->GetPosition(); - b3Vec3 v3 = p3->GetPosition(); - - b3Vec3 B = GetPointB(); - - float32 wABC[4]; - b3BarycentricCoordinates(wABC, v1, v2, v3, B); - - if (wABC[3] > B3_EPSILON) - { - m_u = wABC[0] / wABC[3]; - m_v = wABC[1] / wABC[3]; - } - else - { - m_u = m_v = 0.0f; - } - - if (m_spring) - { - b3ParticleDef pd; - pd.type = e_staticParticle; - pd.position = B; - - m_particle = m_cloth->CreateParticle(pd); - - { - b3SpringForceDef sfd; - sfd.p1 = m_particle; - sfd.p2 = p1; - sfd.restLength = 0.0f; - sfd.structural = 10000.0f; - m_s1 = (b3SpringForce*)m_cloth->CreateForce(sfd); - } - - { - b3SpringForceDef sfd; - sfd.p1 = m_particle; - sfd.p2 = p2; - sfd.restLength = 0.0f; - sfd.structural = 10000.0f; - m_s2 = (b3SpringForce*)m_cloth->CreateForce(sfd); - } - - { - b3SpringForceDef sfd; - sfd.p1 = m_particle; - sfd.p2 = p3; - sfd.restLength = 0.0f; - sfd.structural = 10000.0f; - m_s3 = (b3SpringForce*)m_cloth->CreateForce(sfd); - } - } - else - { - m_t1 = p1->GetType(); - p1->SetType(e_staticParticle); - - m_t2 = p2->GetType(); - p2->SetType(e_staticParticle); - - m_t3 = p3->GetType(); - p3->SetType(e_staticParticle); - } - - return true; - } - - void Drag() - { - B3_ASSERT(IsDragging() == true); - - b3Vec3 A = GetPointA(); - b3Vec3 B = GetPointB(); - - b3Vec3 dx = B - A; - - if (m_spring) - { - m_particle->SetPosition(B); - } - else - { - b3Particle* p1 = m_cloth->GetVertexParticle(m_triangle->v1); - p1->ApplyTranslation(dx); - - b3Particle* p2 = m_cloth->GetVertexParticle(m_triangle->v2); - p2->ApplyTranslation(dx); - - b3Particle* p3 = m_cloth->GetVertexParticle(m_triangle->v3); - p3->ApplyTranslation(dx); - } - } - - void StopDragging() - { - B3_ASSERT(IsDragging() == true); - - if (m_spring) - { - m_cloth->DestroyForce(m_s1); - m_cloth->DestroyForce(m_s2); - m_cloth->DestroyForce(m_s3); - m_cloth->DestroyParticle(m_particle); - } - else - { - m_cloth->GetVertexParticle(m_triangle->v1)->SetType(m_t1); - m_cloth->GetVertexParticle(m_triangle->v2)->SetType(m_t2); - m_cloth->GetVertexParticle(m_triangle->v3)->SetType(m_t3); - } - - m_cloth = nullptr; - } - - b3Vec3 GetPointA() const - { - B3_ASSERT(IsDragging() == true); - - b3Vec3 A = m_cloth->GetVertexParticle(m_triangle->v1)->GetPosition(); - b3Vec3 B = m_cloth->GetVertexParticle(m_triangle->v2)->GetPosition(); - b3Vec3 C = m_cloth->GetVertexParticle(m_triangle->v3)->GetPosition(); - - return m_u * A + m_v * B + (1.0f - m_u - m_v) * C; - } - - b3Vec3 GetPointB() const - { - B3_ASSERT(IsDragging() == true); - - return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); - } -private: - b3Ray3* m_ray; - float32 m_x; - - b3World* m_world; - - b3Cloth* m_cloth; - const b3ClothMesh* m_mesh; - b3ClothMeshTriangle* m_triangle; - float32 m_u, m_v; - - bool m_spring; - - b3Particle* m_particle; - b3SpringForce* m_s1; - b3SpringForce* m_s2; - b3SpringForce* m_s3; - - b3ParticleType m_t1, m_t2, m_t3; -}; - -#endif \ No newline at end of file diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index 692201b..b945ee8 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -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; + friend class b3ClothSolver; + friend class b3ClothContactSolver; + + friend class b3SoftBody; + friend class b3SoftBodySolver; + friend class b3SoftBodyContactSolver; + enum b3BodyFlags { e_awakeFlag = 0x0001, diff --git a/include/bounce/dynamics/cloth/bend_force.h b/include/bounce/dynamics/cloth/bend_force.h deleted file mode 100644 index 069a88e..0000000 --- a/include/bounce/dynamics/cloth/bend_force.h +++ /dev/null @@ -1,158 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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_BEND_FORCE_H -#define B3_BEND_FORCE_H - -#include - -struct b3BendForceDef : public b3ForceDef -{ - b3BendForceDef() - { - type = e_bendForce; - p1 = nullptr; - p2 = nullptr; - p3 = nullptr; - p4 = nullptr; - restDistance = 0.0f; - restAngle = 0.0f; - structural = 0.0f; - damping = 0.0f; - } - - // - void Initialize(b3Particle* particle1, b3Particle* particle2, b3Particle* particle3, b3Particle* particle4, - float32 structuralStiffness, float32 dampingStiffness); - - // Particle 1 - b3Particle* p1; - - // Particle 2 - b3Particle* p2; - - // Particle 3 - b3Particle* p3; - - // Particle 4 - b3Particle* p4; - - // Rest distance - float32 restDistance; - - // Rest angle - float32 restAngle; - - // Structural stiffness - float32 structural; - - // Damping stiffness - float32 damping; -}; - -// -class b3BendForce : public b3Force -{ -public: - b3Particle* GetParticle1(); - - b3Particle* GetParticle2(); - - b3Particle* GetParticle3(); - - b3Particle* GetParticle4(); - - float32 GetRestDistance() const; - - float32 GetRestAngle() const; - - float32 GetStructuralStiffness() const; - - float32 GetDampingStiffness() const; -private: - friend class b3Force; - friend class b3Cloth; - - b3BendForce(const b3BendForceDef* def); - ~b3BendForce(); - - void Apply(const b3ClothSolverData* data); - - // Solver shared - - // Particle 1 - b3Particle* m_p1; - - // Particle 2 - b3Particle* m_p2; - - // Particle 3 - b3Particle* m_p3; - - // Particle 4 - b3Particle* m_p4; - - // Rest distance - float32 m_L0; - - // Rest angle - float32 m_angle0; - - // Structural stiffness - float32 m_ks; - - // Structural stiffness - float32 m_kd; -}; - -inline b3Particle* b3BendForce::GetParticle1() -{ - return m_p1; -} - -inline b3Particle* b3BendForce::GetParticle2() -{ - return m_p2; -} - -inline b3Particle* b3BendForce::GetParticle3() -{ - return m_p3; -} - -inline b3Particle* b3BendForce::GetParticle4() -{ - return m_p4; -} - -inline float32 b3BendForce::GetRestAngle() const -{ - return m_angle0; -} - -inline float32 b3BendForce::GetStructuralStiffness() const -{ - return m_ks; -} - -inline float32 b3BendForce::GetDampingStiffness() const -{ - return m_kd; -} - -#endif \ No newline at end of file diff --git a/include/bounce/dynamics/contact_manager.h b/include/bounce/dynamics/contact_manager.h index a32ffc9..5be9470 100644 --- a/include/bounce/dynamics/contact_manager.h +++ b/include/bounce/dynamics/contact_manager.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/collide/clip.h b/include/bounce/dynamics/contacts/collide/clip.h index d87ccbb..ced23e1 100644 --- a/include/bounce/dynamics/contacts/collide/clip.h +++ b/include/bounce/dynamics/contacts/collide/clip.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/collide/collide.h b/include/bounce/dynamics/contacts/collide/collide.h index 12ec99a..f3741b9 100644 --- a/include/bounce/dynamics/contacts/collide/collide.h +++ b/include/bounce/dynamics/contacts/collide/collide.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/contact.h b/include/bounce/dynamics/contacts/contact.h index 3988f38..c934c5f 100644 --- a/include/bounce/dynamics/contacts/contact.h +++ b/include/bounce/dynamics/contacts/contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/contact_cluster.h b/include/bounce/dynamics/contacts/contact_cluster.h index 4d316e9..f2a0de7 100644 --- a/include/bounce/dynamics/contacts/contact_cluster.h +++ b/include/bounce/dynamics/contacts/contact_cluster.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/contact_solver.h b/include/bounce/dynamics/contacts/contact_solver.h index e8d0c6c..51a3f4e 100644 --- a/include/bounce/dynamics/contacts/contact_solver.h +++ b/include/bounce/dynamics/contacts/contact_solver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/convex_contact.h b/include/bounce/dynamics/contacts/convex_contact.h index 22eb9df..0610b3e 100644 --- a/include/bounce/dynamics/contacts/convex_contact.h +++ b/include/bounce/dynamics/contacts/convex_contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/manifold.h b/include/bounce/dynamics/contacts/manifold.h index 66b4d47..b47e763 100644 --- a/include/bounce/dynamics/contacts/manifold.h +++ b/include/bounce/dynamics/contacts/manifold.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/contacts/mesh_contact.h b/include/bounce/dynamics/contacts/mesh_contact.h index 2b3df78..0a626e4 100644 --- a/include/bounce/dynamics/contacts/mesh_contact.h +++ b/include/bounce/dynamics/contacts/mesh_contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/island.h b/include/bounce/dynamics/island.h index efdb6e0..2a74da7 100644 --- a/include/bounce/dynamics/island.h +++ b/include/bounce/dynamics/island.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joint_manager.h b/include/bounce/dynamics/joint_manager.h index af66e73..d399bb2 100644 --- a/include/bounce/dynamics/joint_manager.h +++ b/include/bounce/dynamics/joint_manager.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/cone_joint.h b/include/bounce/dynamics/joints/cone_joint.h index 663d8aa..1e092b7 100644 --- a/include/bounce/dynamics/joints/cone_joint.h +++ b/include/bounce/dynamics/joints/cone_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/joint.h b/include/bounce/dynamics/joints/joint.h index f973d99..6130f3f 100644 --- a/include/bounce/dynamics/joints/joint.h +++ b/include/bounce/dynamics/joints/joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/joint_solver.h b/include/bounce/dynamics/joints/joint_solver.h index 4ccb9d5..ef36fa9 100644 --- a/include/bounce/dynamics/joints/joint_solver.h +++ b/include/bounce/dynamics/joints/joint_solver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/mouse_joint.h b/include/bounce/dynamics/joints/mouse_joint.h index e044495..a2a35e0 100644 --- a/include/bounce/dynamics/joints/mouse_joint.h +++ b/include/bounce/dynamics/joints/mouse_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/revolute_joint.h b/include/bounce/dynamics/joints/revolute_joint.h index 336c5c9..12e707d 100644 --- a/include/bounce/dynamics/joints/revolute_joint.h +++ b/include/bounce/dynamics/joints/revolute_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/sphere_joint.h b/include/bounce/dynamics/joints/sphere_joint.h index 086b366..16d82ab 100644 --- a/include/bounce/dynamics/joints/sphere_joint.h +++ b/include/bounce/dynamics/joints/sphere_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/spring_joint.h b/include/bounce/dynamics/joints/spring_joint.h index cbd16e2..32f57db 100644 --- a/include/bounce/dynamics/joints/spring_joint.h +++ b/include/bounce/dynamics/joints/spring_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/joints/weld_joint.h b/include/bounce/dynamics/joints/weld_joint.h index e6ced48..29cfb47 100644 --- a/include/bounce/dynamics/joints/weld_joint.h +++ b/include/bounce/dynamics/joints/weld_joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/shapes/capsule_shape.h b/include/bounce/dynamics/shapes/capsule_shape.h index 579e48f..2c3d089 100644 --- a/include/bounce/dynamics/shapes/capsule_shape.h +++ b/include/bounce/dynamics/shapes/capsule_shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/shapes/hull_shape.h b/include/bounce/dynamics/shapes/hull_shape.h index 193798f..7cc3f7e 100644 --- a/include/bounce/dynamics/shapes/hull_shape.h +++ b/include/bounce/dynamics/shapes/hull_shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/shapes/mesh_shape.h b/include/bounce/dynamics/shapes/mesh_shape.h index c8215e6..e75775c 100644 --- a/include/bounce/dynamics/shapes/mesh_shape.h +++ b/include/bounce/dynamics/shapes/mesh_shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -41,6 +41,8 @@ public: bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const; + bool TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf, u32 childIndex) const; + bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const; bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf, u32 childIndex) const; diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index 53aa35e..b397f58 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -140,6 +140,9 @@ public: // Set the user data associated with this shape. void SetUserData(void* data); + // Get broadphase AABB + const b3AABB3& GetAABB() const; + // Dump this shape to the log file. void Dump(u32 bodyIndex) const; @@ -153,6 +156,7 @@ protected: friend class b3Body; friend class b3Contact; friend class b3ContactManager; + friend class b3MeshShape; friend class b3MeshContact; friend class b3ContactSolver; friend class b3List1; diff --git a/include/bounce/dynamics/shapes/sphere_shape.h b/include/bounce/dynamics/shapes/sphere_shape.h index be5514b..7e398d6 100644 --- a/include/bounce/dynamics/shapes/sphere_shape.h +++ b/include/bounce/dynamics/shapes/sphere_shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/time_step.h b/include/bounce/dynamics/time_step.h index 670c074..6736ffe 100644 --- a/include/bounce/dynamics/time_step.h +++ b/include/bounce/dynamics/time_step.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/world.h b/include/bounce/dynamics/world.h index 2d30247..a60d72d 100644 --- a/include/bounce/dynamics/world.h +++ b/include/bounce/dynamics/world.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,21 +22,20 @@ #include #include #include +#include #include #include #include -struct b3ClothDef; struct b3BodyDef; -class b3Cloth; class b3Body; class b3QueryListener; class b3RayCastListener; class b3ContactListener; class b3ContactFilter; -struct b3RayCastSingleShapeOutput +struct b3RayCastSingleOutput { b3Shape* shape; // shape b3Vec3 point; // intersection point on surface @@ -44,15 +43,6 @@ struct b3RayCastSingleShapeOutput float32 fraction; // time of intersection on segment }; -struct b3RayCastSingleClothOutput -{ - b3Cloth* cloth; // cloth - u32 triangle; // triangle - b3Vec3 point; // intersection point on surface - b3Vec3 normal; // surface normal of intersection - float32 fraction; // time of intersection on segment -}; - // Use a physics world to create/destroy rigid bodies, execute ray cast and volume queries. class b3World { @@ -79,12 +69,6 @@ public: // The acceleration has units of m/s^2. void SetGravity(const b3Vec3& gravity); - // Create a new deformable cloth. - b3Cloth* CreateCloth(const b3ClothDef& def); - - // Destroy an existing deformable cloth. - void DestroyCloth(b3Cloth* cloth); - // Create a new rigid body. b3Body* CreateBody(const b3BodyDef& def); @@ -108,29 +92,14 @@ public: // The ray cast output is the intercepted shape, the intersection // point in world space, the face normal on the shape associated with the point, // and the intersection fraction. - void RayCastShape(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const; + void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const; // Perform a ray cast with the world. // If the ray doesn't intersect with a shape in the world then return false. // The ray cast output is the intercepted shape, the intersection // point in world space, the face normal on the shape associated with the point, // and the intersection fraction. - bool RayCastSingleShape(b3RayCastSingleShapeOutput* output, const b3Vec3& p1, const b3Vec3& p2) const; - - // Perform a ray cast with the world. - // The given ray cast listener will be notified when a ray intersects a shape - // in the world. - // The ray cast output is the intercepted cloth, the intersection - // point in world space, the face normal on the cloth associated with the point, - // and the intersection fraction. - void RayCastCloth(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const; - - // Perform a ray cast with the world. - // If the ray doesn't intersect with a cloth in the world then return false. - // The ray cast output is the intercepted cloth, the intersection - // point in world space, the face normal on the cloth associated with the point, - // and the intersection fraction. - bool RayCastSingleCloth(b3RayCastSingleClothOutput* output, const b3Vec3& p1, const b3Vec3& p2) const; + bool RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const; // Perform a AABB query with the world. // The query listener will be notified when two shape AABBs are overlapping. @@ -153,16 +122,21 @@ public: // Draw the entities in this world. void Draw() const; + // Draw solid the entities in this world. + void DrawSolid() const; + // Draw a shape. - void DrawShape(const b3Transform& xf, const b3Shape* shape) const; -private : + void DrawShape(const b3Transform& xf, const b3Shape* shape, const b3Color& color) const; + + // Draw solid a shape. + void DrawSolidShape(const b3Transform& xf, const b3Shape* shape, const b3Color& color) const; +private: enum b3Flags { e_shapeAddedFlag = 0x0001, e_clearForcesFlag = 0x0002, }; - - friend class b3Cloth; + friend class b3Body; friend class b3Shape; friend class b3Contact; @@ -172,20 +146,16 @@ private : void Solve(float32 dt, u32 velocityIterations, u32 positionIterations); - void StepCloth(float32 dt); - bool m_sleeping; bool m_warmStarting; u32 m_flags; b3Vec3 m_gravity; b3StackAllocator m_stackAllocator; - b3BlockPool m_clothBlocks; + + // Pool of bodies b3BlockPool m_bodyBlocks; - // List of clothes - b3List2 m_clothList; - // List of bodies b3List2 m_bodyList; diff --git a/include/bounce/dynamics/world_listeners.h b/include/bounce/dynamics/world_listeners.h index cc9fc02..275f505 100644 --- a/include/bounce/dynamics/world_listeners.h +++ b/include/bounce/dynamics/world_listeners.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -22,7 +22,6 @@ #include class b3Shape; -class b3Cloth; class b3Contact; class b3QueryListener @@ -47,12 +46,6 @@ public: // the intersection point on the shape, the surface normal associated with the point, and the // intersection fraction for the ray. virtual float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction) = 0; - - // Report that a cloth was hit by the ray to this contact listener. - // The reported information are the shape hit by the ray, - // the intersection point on the cloth, the surface normal associated with the point, the - // intersection fraction for the ray, and the triangle. - virtual float32 ReportCloth(b3Cloth* cloth, const b3Vec3& point, const b3Vec3& normal, float32 fraction, u32 triangle) = 0; }; class b3ContactListener diff --git a/include/bounce/garment/garment.h b/include/bounce/garment/garment.h deleted file mode 100644 index 742ace0..0000000 --- a/include/bounce/garment/garment.h +++ /dev/null @@ -1,166 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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_GARMENT_H -#define B3_GARMENT_H - -#include - -struct b3SewingLine -{ - u32 p1, p2; - u32 v1, v2; -}; - -struct b3Garment -{ - u32 patternCount; - b3SewingPattern** patterns; - u32 sewingCount; - b3SewingLine* sewingLines; -}; - -struct b3RectangleGarment : public b3Garment -{ - b3RectanglePattern rectangle; - b3SewingPattern* rectanglePatterns[1]; - - b3RectangleGarment(float32 ex = 1.0f, float32 ey = 1.0f) : rectangle(ex, ey) - { - rectanglePatterns[0] = &rectangle; - - patternCount = 1; - patterns = rectanglePatterns; - - sewingCount = 0; - sewingLines = nullptr; - } -}; - -struct b3CircleGarment : public b3Garment -{ - b3CirclePattern<> circle; - b3SewingPattern* circlePatterns[1]; - - b3CircleGarment(float32 r = 1.0f) : circle(r) - { - circlePatterns[0] = &circle; - - patternCount = 1; - patterns = circlePatterns; - - sewingCount = 0; - sewingLines = nullptr; - } -}; - -struct b3ShirtGarment : public b3Garment -{ - b3RectanglePattern frontBody; - b3RectanglePattern frontRightSleeve; - b3RectanglePattern frontLeftSleeve; - - b3RectanglePattern backBody; - b3RectanglePattern backRightSleeve; - b3RectanglePattern backLeftSleeve; - - b3SewingLine shirtSewingLines[12]; - - b3SewingPattern* shirtPatterns[6]; - - b3ShirtGarment() : - frontBody(1.0f, 1.0f), - frontRightSleeve(0.2f, 0.2f), - frontLeftSleeve(0.2f, 0.2f), - backBody(1.0f, 1.0f), - backRightSleeve(0.2f, 0.2f), - backLeftSleeve(0.2f, 0.2f) - { - b3Vec2 etr; - etr.x = 1.2f; - etr.y = 0.8f; - - b3Vec2 etl; - etl.x = -1.2f; - etl.y = 0.8f; - - for (u32 i = 0; i < 4; ++i) - { - frontRightSleeve.vertices[i] += etr; - frontLeftSleeve.vertices[i] += etl; - backRightSleeve.vertices[i] += etr; - backLeftSleeve.vertices[i] += etl; - } - - // Perform sewing - u32 count = 0; - - // Sew bodies - for (u32 i = 0; i < frontBody.vertexCount; ++i) - { - b3SewingLine line; - line.p1 = 0; - line.p2 = 3; - line.v1 = i; - line.v2 = i; - - shirtSewingLines[count++] = line; - } - - // Sew sleeves - for (u32 i = 0; i < frontRightSleeve.vertexCount; ++i) - { - b3SewingLine line; - line.p1 = 1; - line.p2 = 4; - line.v1 = i; - line.v2 = i; - - shirtSewingLines[count++] = line; - } - - for (u32 i = 0; i < frontLeftSleeve.vertexCount; ++i) - { - b3SewingLine line; - line.p1 = 2; - line.p2 = 5; - line.v1 = i; - line.v2 = i; - - shirtSewingLines[count++] = line; - } - - B3_ASSERT(12 == count); - - shirtPatterns[0] = &frontBody; - shirtPatterns[1] = &frontRightSleeve; - shirtPatterns[2] = &frontLeftSleeve; - - shirtPatterns[3] = &backBody; - shirtPatterns[4] = &backRightSleeve; - shirtPatterns[5] = &backLeftSleeve; - - patternCount = 6; - patterns = shirtPatterns; - - sewingCount = 12; - sewingLines = shirtSewingLines; - } -}; - -#endif \ No newline at end of file diff --git a/include/bounce/meshgen/cylinder_mesh.h b/include/bounce/meshgen/cylinder_mesh.h new file mode 100644 index 0000000..f1d0653 --- /dev/null +++ b/include/bounce/meshgen/cylinder_mesh.h @@ -0,0 +1,54 @@ +/* +* 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 CYM_MESH_H +#define CYM_MESH_H + +#include + +// This structure represents a triangle mesh. +struct cymMesh +{ + cymMesh() + { + vertexCount = 0; + vertices = nullptr; + normals = nullptr; + indexCount = 0; + indices = nullptr; + } + + ~cymMesh() + { + b3Free(vertices); + b3Free(normals); + b3Free(indices); + } + + u32 vertexCount; // number of unique vertices + b3Vec3* vertices; // list of unique vertices + b3Vec3* normals; // list of vertex normals + u32 indexCount; // number of triangle vertex indices + u32* indices; // list of triangle vertex index +}; + +// Create a unit cylinder along the y-axis given the number of segments. +// The number of segments must be greater than 2. +void cymCreateMesh(cymMesh& output, u32 segments); + +#endif \ No newline at end of file diff --git a/include/bounce/meshgen/sphere_mesh.h b/include/bounce/meshgen/sphere_mesh.h new file mode 100644 index 0000000..fbcbc9d --- /dev/null +++ b/include/bounce/meshgen/sphere_mesh.h @@ -0,0 +1,54 @@ +/* +* 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 SM_MESH_H +#define SM_MESH_H + +#include + +// This structure represents a triangle mesh. +struct smMesh +{ + smMesh() + { + vertexCount = 0; + vertices = nullptr; + normals = nullptr; + indexCount = 0; + indices = nullptr; + } + + ~smMesh() + { + b3Free(vertices); + b3Free(normals); + b3Free(indices); + } + + u32 vertexCount; // number of unique vertices + b3Vec3* vertices; // list of unique vertices + b3Vec3* normals; // list of vertex normals + u32 indexCount; // number of triangle vertex indices + u32* indices; // list of triangle vertex index +}; + +// Create a unit icosphere given the number of subdivisions. +// If the number of subdivisions to perform is zero then the output mesh is an octahedron. +void smCreateMesh(smMesh& output, u32 subdivisions); + +#endif \ No newline at end of file diff --git a/include/bounce/quickhull/qh_hull.h b/include/bounce/quickhull/qh_hull.h index 108f236..877097f 100644 --- a/include/bounce/quickhull/qh_hull.h +++ b/include/bounce/quickhull/qh_hull.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/rope/rope.h b/include/bounce/rope/rope.h similarity index 95% rename from include/bounce/dynamics/rope/rope.h rename to include/bounce/rope/rope.h index 4e04763..92e782a 100644 --- a/include/bounce/dynamics/rope/rope.h +++ b/include/bounce/rope/rope.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/dynamics/spatial.h b/include/bounce/rope/spatial.h similarity index 98% rename from include/bounce/dynamics/spatial.h rename to include/bounce/rope/spatial.h index 2c944ca..a1b6bd6 100644 --- a/include/bounce/dynamics/spatial.h +++ b/include/bounce/rope/spatial.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/include/bounce/softbody/block_softbody_mesh.h b/include/bounce/softbody/block_softbody_mesh.h new file mode 100644 index 0000000..19dba1e --- /dev/null +++ b/include/bounce/softbody/block_softbody_mesh.h @@ -0,0 +1,123 @@ +/* +* 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_BLOCK_SOFT_BODY_MESH_H +#define B3_BLOCK_SOFT_BODY_MESH_H + +#include + +template +struct b3BlockSoftBodyMesh : public b3SoftBodyMesh +{ + b3Vec3 blockVertices[(W + 1) * (H + 1) * (D + 1)]; + b3SoftBodyMeshTetrahedron blockTetrahedrons[5 * W * H * D]; + + b3BlockSoftBodyMesh() + { + vertexCount = 0; + for (u32 x = 0; x <= W; ++x) + { + for (u32 y = 0; y <= H; ++y) + { + for (u32 z = 0; z <= D; ++z) + { + blockVertices[vertexCount++].Set(float32(x), float32(y), float32(z)); + } + } + } + + B3_ASSERT(vertexCount == (W + 1) * (H + 1) * (D + 1)); + + b3Vec3 translation; + translation.x = -0.5f * float32(W); + translation.y = -0.5f * float32(H); + translation.z = -0.5f * float32(D); + + for (u32 i = 0; i < vertexCount; ++i) + { + blockVertices[i] += translation; + } + + tetrahedronCount = 0; + for (u32 x = 0; x < W; ++x) + { + for (u32 y = 0; y < H; ++y) + { + for (u32 z = 0; z < D; ++z) + { + // 4*-----*7 + // /| /| + // / | / | + // 5*-----*6 | + // | 0*--|--*3 + // | / | / + // |/ |/ + // 1*-----*2 + u32 v0 = (x * (H + 1) + y) * (D + 1) + z; + u32 v1 = v0 + 1; + u32 v3 = ((x + 1) * (H + 1) + y) * (D + 1) + z; + u32 v2 = v3 + 1; + u32 v7 = ((x + 1) * (H + 1) + (y + 1)) * (D + 1) + z; + u32 v6 = v7 + 1; + u32 v4 = (x * (H + 1) + (y + 1)) * (D + 1) + z; + u32 v5 = v4 + 1; + + if ((x + y + z) % 2 == 1) + { + // CCW + //blockTetrahedrons[tetrahedronCount++] = { v1, v2, v6, v3 }; + //blockTetrahedrons[tetrahedronCount++] = { v3, v6, v4, v7 }; + //blockTetrahedrons[tetrahedronCount++] = { v1, v4, v6, v5 }; + //blockTetrahedrons[tetrahedronCount++] = { v1, v3, v4, v0 }; + //blockTetrahedrons[tetrahedronCount++] = { v1, v6, v4, v3 }; + + // CW + blockTetrahedrons[tetrahedronCount++] = { v2, v1, v6, v3 }; + blockTetrahedrons[tetrahedronCount++] = { v6, v3, v4, v7 }; + blockTetrahedrons[tetrahedronCount++] = { v4, v1, v6, v5 }; + blockTetrahedrons[tetrahedronCount++] = { v3, v1, v4, v0 }; + blockTetrahedrons[tetrahedronCount++] = { v6, v1, v4, v3 }; + } + else + { + // CCW + //blockTetrahedrons[tetrahedronCount++] = { v2, v0, v5, v1 }; + //blockTetrahedrons[tetrahedronCount++] = { v2, v7, v0, v3 }; + //blockTetrahedrons[tetrahedronCount++] = { v2, v5, v7, v6 }; + //blockTetrahedrons[tetrahedronCount++] = { v0, v7, v5, v4 }; + //blockTetrahedrons[tetrahedronCount++] = { v2, v0, v7, v5 }; + + // CW + blockTetrahedrons[tetrahedronCount++] = { v0, v2, v5, v1 }; + blockTetrahedrons[tetrahedronCount++] = { v7, v2, v0, v3 }; + blockTetrahedrons[tetrahedronCount++] = { v5, v2, v7, v6 }; + blockTetrahedrons[tetrahedronCount++] = { v7, v0, v5, v4 }; + blockTetrahedrons[tetrahedronCount++] = { v0, v2, v7, v5 }; + } + } + } + } + + B3_ASSERT(tetrahedronCount == 5 * W * H * D); + + vertices = blockVertices; + tetrahedrons = blockTetrahedrons; + } +}; + +#endif \ No newline at end of file diff --git a/include/bounce/softbody/contacts/softbody_contact_solver.h b/include/bounce/softbody/contacts/softbody_contact_solver.h new file mode 100644 index 0000000..4187ab4 --- /dev/null +++ b/include/bounce/softbody/contacts/softbody_contact_solver.h @@ -0,0 +1,127 @@ +/* +* 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 +#include + +class b3StackAllocator; + +struct b3SoftBodyNode; +class b3Body; + +struct 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; + + b3Vec3* positions; + b3Vec3* velocities; + + u32 bodyContactCount; + b3NodeBodyContact** bodyContacts; +}; + +inline float32 b3MixFriction(float32 u1, float32 u2) +{ + return b3Sqrt(u1 * u2); +} + +class b3SoftBodyContactSolver +{ +public: + b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def); + ~b3SoftBodyContactSolver(); + + void InitializeBodyContactConstraints(); + + void WarmStart(); + + void SolveBodyContactVelocityConstraints(); + + void StoreImpulses(); + + bool SolveBodyContactPositionConstraints(); +protected: + b3StackAllocator* m_allocator; + + b3Vec3* m_positions; + b3Vec3* m_velocities; + + u32 m_bodyContactCount; + b3NodeBodyContact** m_bodyContacts; + + b3SoftBodySolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; + b3SoftBodySolverBodyContactPositionConstraint* m_bodyPositionConstraints; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/softbody/contacts/softbody_node_body_contact.h b/include/bounce/softbody/contacts/softbody_node_body_contact.h new file mode 100644 index 0000000..8a8bb5e --- /dev/null +++ b/include/bounce/softbody/contacts/softbody_node_body_contact.h @@ -0,0 +1,78 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_SOFT_BODY_NODE_BODY_CONTACT_H +#define B3_SOFT_BODY_NODE_BODY_CONTACT_H + +#include +#include +#include +#include + +struct b3SoftBodyNode; +class b3Shape; + +// A contact between a node and a body +class b3NodeBodyContact +{ +public: +private: + friend class b3List2; + friend class b3SoftBody; + friend class b3SoftBodyContactManager; + friend struct b3SoftBodyNode; + friend class b3SoftBodySolver; + friend class b3SoftBodyContactSolver; + friend struct b3NodeBodyContactWorldPoint; + + b3NodeBodyContact() { } + ~b3NodeBodyContact() { } + + void Update(); + + b3SoftBodyNode* m_n1; + b3Shape* m_s2; + + // Is the contact active? + bool m_active; + + // Contact constraint + b3Vec3 m_normal1; + b3Vec3 m_localPoint1; + b3Vec3 m_localPoint2; + float32 m_normalImpulse; + + // Friction constraint + b3Vec3 m_tangent1, m_tangent2; + b3Vec2 m_tangentImpulse; + + // List pointers into the soft body + b3NodeBodyContact* m_prev; + b3NodeBodyContact* m_next; +}; + +struct b3NodeBodyContactWorldPoint +{ + void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + + b3Vec3 point; + b3Vec3 normal; + float32 separation; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/softbody/softbody.h b/include/bounce/softbody/softbody.h new file mode 100644 index 0000000..f3394c7 --- /dev/null +++ b/include/bounce/softbody/softbody.h @@ -0,0 +1,217 @@ +/* +* 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 +#include + +class b3World; + +struct b3SoftBodyMesh; + +struct b3SoftBodyNode; +struct b3SoftBodyElement; + +struct b3RayCastInput; +struct b3RayCastOutput; + +struct b3SoftBodyRayCastSingleOutput +{ + u32 tetrahedron; + u32 v1, v2, v3; + float32 fraction; + b3Vec3 normal; +}; + +// Soft body tetrahedron element +struct b3SoftBodyElement +{ + float32 E; + float32 nu; + + float32 c_yield; + float32 c_creep; + float32 c_max; + + b3Mat33 K[16]; // 12 x 12 + b3Mat33 invE; + b3Quat q; + float32 B[72]; // 6 x 12 + float32 P[72]; // V * BT * E -> 12 x 6 + float32 epsilon_plastic[6]; // 6 x 1 +}; + +// 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 +// and some uniform material parameters. +struct b3SoftBodyDef +{ + b3SoftBodyDef() + { + mesh = nullptr; + density = 0.1f; + E = 100.0f; + nu = 0.3f; + c_yield = B3_MAX_FLOAT; + c_creep = 0.0f; + c_max = 0.0f; + } + + // 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; + + // Material elastic strain yield in [0, inf] + // This is a dimensionless value + float32 c_yield; + + // Material creep rate in [0, 1 / dt] + // Units are Hz + float32 c_creep; + + // Material maximum plastic strain in [0, inf] + // This is a dimensionless value + float32 c_max; +}; + +// A soft body represents a deformable volume as a collection of nodes and elements. +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 soft body. + // The soft body will be able to respond to collisions with the bodies in the attached world. + void SetWorld(b3World* world); + + // Get the world attached to this soft body. + 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 element associated with the given tetrahedron. + b3SoftBodyElement* GetTetrahedronElement(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: + friend class b3SoftBodyContactManager; + friend class b3SoftBodySolver; + friend class b3SoftBodyForceSolver; + friend struct b3SoftBodyNode; + + // Compute mass of each node. + void ComputeMass(); + + // 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; + + // Soft body nodes + b3SoftBodyNode* m_nodes; + + // Soft body elements + b3SoftBodyElement* m_elements; + + // Soft body triangles + b3SoftBodyTriangle* m_triangles; + + // Contact manager + b3SoftBodyContactManager m_contactManager; + + // Attached world + b3World* m_world; +}; + +inline void b3SoftBody::SetGravity(const b3Vec3& gravity) +{ + m_gravity = gravity; +} + +inline b3Vec3 b3SoftBody::GetGravity() const +{ + return m_gravity; +} + +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 \ No newline at end of file diff --git a/include/bounce/softbody/softbody_contact_manager.h b/include/bounce/softbody/softbody_contact_manager.h new file mode 100644 index 0000000..5e57310 --- /dev/null +++ b/include/bounce/softbody/softbody_contact_manager.h @@ -0,0 +1,48 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_SOFTBODY_CONTACT_MANAGER_H +#define B3_SOFTBODY_CONTACT_MANAGER_H + +#include +#include +#include +#include + +class b3SoftBody; + +// Contact delegator for b3SoftBody. +class b3SoftBodyContactManager +{ +public: + b3SoftBodyContactManager(); + + void FindNewBodyContacts(); + void AddNSPair(b3SoftBodyNode* n1, b3Shape* s2); + void UpdateBodyContacts(); + + b3NodeBodyContact* CreateNodeBodyContact(); + void Destroy(b3NodeBodyContact* c); + + b3BlockPool m_nodeBodyContactBlocks; + b3SoftBody* m_body; + b3BroadPhase m_broadPhase; + b3List2 m_nodeBodyContactList; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/framework/recorder_profiler.h b/include/bounce/softbody/softbody_force_solver.h similarity index 53% rename from examples/testbed/framework/recorder_profiler.h rename to include/bounce/softbody/softbody_force_solver.h index 5291901..1c2f35b 100644 --- a/examples/testbed/framework/recorder_profiler.h +++ b/include/bounce/softbody/softbody_force_solver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,37 +16,38 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef RECORDER_PROFILER_H -#define RECORDER_PROFILER_H +#ifndef B3_SOFT_BODY_FORCE_SOLVER_H +#define B3_SOFT_BODY_FORCE_SOLVER_H -#include -#include +#include +#include -// An event in the profiler event recorder. -struct ProfilerRecord +class b3StackAllocator; + +class b3SoftBody; +class b3SoftBodyMesh; + +struct b3SoftBodyNode; +struct b3SoftBodyElement; + +struct b3SoftBodyForceSolverDef { - float64 elapsed; - float64 maxElapsed; - const char* name; + b3SoftBody* body; }; -// The profiler recorder simply keeps profile events in an event buffer, -// so that they can be rendered, saved to a file, etc. later in a -// particular position in code. -class RecorderProfiler +class b3SoftBodyForceSolver { public: - void BeginEvents(); + b3SoftBodyForceSolver(const b3SoftBodyForceSolverDef& def); + ~b3SoftBodyForceSolver(); - void EndEvents(); - - void Add(const char* name, float64 elapsedTime); - - const b3Array& GetRecords() const { return m_records; } + void Solve(float32 dt, const b3Vec3& gravity); private: - b3StackArray m_records; + b3SoftBody* m_body; + b3StackAllocator* m_allocator; + const b3SoftBodyMesh* m_mesh; + b3SoftBodyNode* m_nodes; + b3SoftBodyElement* m_elements; }; -extern RecorderProfiler* g_profilerRecorder; - #endif \ No newline at end of file diff --git a/examples/testbed/framework/recorder_profiler.cpp b/include/bounce/softbody/softbody_mesh.h similarity index 55% rename from examples/testbed/framework/recorder_profiler.cpp rename to include/bounce/softbody/softbody_mesh.h index 8b126cb..4ffc686 100644 --- a/examples/testbed/framework/recorder_profiler.cpp +++ b/include/bounce/softbody/softbody_mesh.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,39 +16,32 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#ifndef B3_SOFT_BODY_MESH_H +#define B3_SOFT_BODY_MESH_H -RecorderProfiler* g_profilerRecorder = nullptr; +#include -void RecorderProfiler::BeginEvents() +struct b3SoftBodyMeshTetrahedron { - for (u32 i = 0; i < m_records.Count(); ++i) - { - m_records[i].elapsed = 0.0; - } -} + u32 v1, v2, v3, v4; +}; -void RecorderProfiler::EndEvents() +struct b3SoftBodyMesh { -} + u32 vertexCount; + b3Vec3* vertices; + u32 tetrahedronCount; + b3SoftBodyMeshTetrahedron* tetrahedrons; +}; -void RecorderProfiler::Add(const char* name, float64 elapsedTime) +struct b3QSoftBodyMesh : public b3SoftBodyMesh { - for (u32 i = 0; i < m_records.Count(); ++i) - { - ProfilerRecord& r = m_records[i]; - if (r.name == name) - { - r.elapsed += elapsedTime; - r.maxElapsed = b3Max(r.maxElapsed, elapsedTime); - return; - } - } + b3QSoftBodyMesh(); + ~b3QSoftBodyMesh(); - ProfilerRecord r; - r.name = name; - r.elapsed = 0.0; - r.maxElapsed = 0.0; + void SetAsSphere(float32 radius, u32 subdivisions); + + void SetAsCylinder(float32 radius, float32 ey, u32 segments); +}; - m_records.PushBack(r); -} \ No newline at end of file +#endif \ No newline at end of file diff --git a/include/bounce/softbody/softbody_node.h b/include/bounce/softbody/softbody_node.h new file mode 100644 index 0000000..8e53062 --- /dev/null +++ b/include/bounce/softbody/softbody_node.h @@ -0,0 +1,249 @@ +/* +* 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 +#include +#include + +class b3SoftBody; + +// Static node: Can be moved manually. +// Kinematic node: Non-zero velocity, can be moved by the solver. +// Dynamic node: Non-zero velocity determined by force, can be moved by the solver. +enum b3SoftBodyNodeType +{ + e_staticSoftBodyNode, + e_kinematicSoftBodyNode, + e_dynamicSoftBodyNode +}; + +// A soft body node. +struct 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 node position. + // If the node is dynamic changing the position directly might lead + // to physically incorrect simulation behaviour. + void SetPosition(const b3Vec3& position); + + // Get the node position. + const b3Vec3& GetPosition() const; + + // Set the node velocity. + void SetVelocity(const b3Vec3& velocity); + + // Get the node velocity. + const b3Vec3& GetVelocity() const; + + // Get the node mass. + float32 GetMass() const; + + // Set the node mass damping. + void SetMassDamping(float32 damping); + + // Get the node mass damping. + float32 GetMassDamping() const; + + // Set the node radius. + void SetRadius(float32 radius); + + // Get the node radius. + float32 GetRadius() const; + + // Set the node coefficient of friction. + void SetFriction(float32 friction); + + // Get the node coefficient of friction. + float32 GetFriction() const; + + // Apply a force. + void ApplyForce(const b3Vec3& force); +private: + friend class b3SoftBody; + friend class b3SoftBodyContactManager; + friend class b3SoftBodySolver; + friend class b3SoftBodyForceSolver; + friend class b3SoftBodyContactSolver; + friend class b3NodeBodyContact; + + b3SoftBodyNode() { } + + ~b3SoftBodyNode() { } + + // Synchronize node + void Synchronize(const b3Vec3& displacement); + + // Destroy associated contacts + void DestroyContacts(); + + // 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; + + // Mass damping + float32 m_massDamping; + + // Radius + float32 m_radius; + + // Coefficient of friction + float32 m_friction; + + // User data. + void* m_userData; + + // Soft body mesh vertex index. + u32 m_vertex; + + // Broadphase proxy + u32 m_broadPhaseId; + + // 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(); + Synchronize(b3Vec3_zero); + } + + DestroyContacts(); +} + +inline b3SoftBodyNodeType b3SoftBodyNode::GetType() const +{ + return m_type; +} + +inline u32 b3SoftBodyNode::GetVertex() const +{ + return m_vertex; +} + +inline void b3SoftBodyNode::SetPosition(const b3Vec3& position) +{ + b3Vec3 displacement = position - m_position; + + m_position = position; + + Synchronize(displacement); +} + +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::SetMassDamping(float32 massDamping) +{ + m_massDamping = massDamping; +} + +inline float32 b3SoftBodyNode::GetMassDamping() const +{ + return m_massDamping; +} + +inline void b3SoftBodyNode::SetRadius(float32 radius) +{ + m_radius = radius; + + Synchronize(b3Vec3_zero); +} + +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 \ No newline at end of file diff --git a/include/bounce/softbody/softbody_solver.h b/include/bounce/softbody/softbody_solver.h new file mode 100644 index 0000000..e0a3396 --- /dev/null +++ b/include/bounce/softbody/softbody_solver.h @@ -0,0 +1,60 @@ +/* +* 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 +#include + +class b3StackAllocator; + +class b3SoftBody; +class b3SoftBodyMesh; + +struct b3SoftBodyNode; +struct b3SoftBodyElement; + +struct b3NodeBodyContact; + +struct b3SoftBodySolverDef +{ + b3SoftBody* body; +}; + +class b3SoftBodySolver +{ +public: + b3SoftBodySolver(const b3SoftBodySolverDef& def); + ~b3SoftBodySolver(); + + void Add(b3NodeBodyContact* c); + + void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); +private: + b3SoftBody* m_body; + b3StackAllocator* m_allocator; + const b3SoftBodyMesh* m_mesh; + b3SoftBodyNode* m_nodes; + b3SoftBodyElement* m_elements; + u32 m_bodyContactCapacity; + u32 m_bodyContactCount; + b3NodeBodyContact** m_bodyContacts; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/dense_vec3.h b/include/bounce/sparse/dense_vec3.h similarity index 94% rename from include/bounce/dynamics/cloth/dense_vec3.h rename to include/bounce/sparse/dense_vec3.h index 34820e1..ffc4e42 100644 --- a/include/bounce/dynamics/cloth/dense_vec3.h +++ b/include/bounce/sparse/dense_vec3.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -171,4 +171,11 @@ inline b3DenseVec3 operator*(float32 a, const b3DenseVec3& b) return result; } +inline b3DenseVec3 operator-(const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + b3Negate(result, v); + return result; +} + #endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/diag_mat33.h b/include/bounce/sparse/diag_mat33.h similarity index 96% rename from include/bounce/dynamics/cloth/diag_mat33.h rename to include/bounce/sparse/diag_mat33.h index 34272af..f7a1486 100644 --- a/include/bounce/dynamics/cloth/diag_mat33.h +++ b/include/bounce/sparse/diag_mat33.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -20,7 +20,7 @@ #define B3_DIAG_MAT_33_H #include -#include +#include // Diagonal matrix storing only the diagonal elements of the // original matrix. diff --git a/include/bounce/dynamics/cloth/sparse_sym_mat33.h b/include/bounce/sparse/sparse_mat33.h similarity index 52% rename from include/bounce/dynamics/cloth/sparse_sym_mat33.h rename to include/bounce/sparse/sparse_mat33.h index e84f69b..7bebd1a 100644 --- a/include/bounce/dynamics/cloth/sparse_sym_mat33.h +++ b/include/bounce/sparse/sparse_mat33.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,24 +16,22 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B3_SPARSE_SYM_MAT_33_H -#define B3_SPARSE_SYM_MAT_33_H +#ifndef B3_SPARSE_MAT_33_H +#define B3_SPARSE_MAT_33_H #include -#include -#include +#include +#include -// An element in a sparse symmetric matrix. +// An element in a sparse matrix. struct b3RowValue { u32 column; b3Mat33 value; - - b3RowValue* prev; b3RowValue* next; }; -// Doubly linked list of row elements. +// Singly linked list of row elements. struct b3RowValueList { b3RowValueList() @@ -46,63 +44,36 @@ struct b3RowValueList void PushFront(b3RowValue* link) { - link->prev = NULL; link->next = head; - if (head) - { - head->prev = link; - } head = link; ++count; } - void PushAfter(b3RowValue* prev, b3RowValue* link) - { - link->prev = prev; - - if (prev->next == NULL) - { - link->next = NULL; - } - else - { - link->next = prev->next; - prev->next->prev = link; - } - - prev->next = link; - - ++count; - } - b3RowValue* head; u32 count; }; - -// A sparse symmetric matrix. +// A sparse matrix. // Each row is a list of non-zero elements in the row. -// The total matrix capacity is bounded by -// M * (M + 1) / 2 -struct b3SparseSymMat33 +struct b3SparseMat33 { // - b3SparseSymMat33(); + b3SparseMat33(); // - b3SparseSymMat33(u32 m); + b3SparseMat33(u32 m); // - b3SparseSymMat33(const b3SparseSymMat33& _m); + b3SparseMat33(const b3SparseMat33& _m); // - ~b3SparseSymMat33(); + ~b3SparseMat33(); // - b3SparseSymMat33& operator=(const b3SparseSymMat33& _m); + b3SparseMat33& operator=(const b3SparseMat33& _m); // - void Copy(const b3SparseSymMat33& _m); + void Copy(const b3SparseMat33& _m); // void Destroy(); @@ -114,25 +85,22 @@ struct b3SparseSymMat33 const b3Mat33& operator()(u32 i, u32 j) const; // - void operator+=(const b3SparseSymMat33& m); + void operator+=(const b3SparseMat33& m); // - void operator-=(const b3SparseSymMat33& m); - - // - void Diagonal(b3DiagMat33& out) const; + void operator-=(const b3SparseMat33& m); u32 rowCount; - b3RowValueList* rows; + b3RowValueList* rows; }; -inline b3SparseSymMat33::b3SparseSymMat33() +inline b3SparseMat33::b3SparseMat33() { rowCount = 0; rows = nullptr; } -inline b3SparseSymMat33::b3SparseSymMat33(u32 m) +inline b3SparseMat33::b3SparseMat33(u32 m) { rowCount = m; rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList)); @@ -142,7 +110,7 @@ inline b3SparseSymMat33::b3SparseSymMat33(u32 m) } } -inline b3SparseSymMat33::b3SparseSymMat33(const b3SparseSymMat33& m) +inline b3SparseMat33::b3SparseMat33(const b3SparseMat33& m) { rowCount = m.rowCount; rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList)); @@ -154,12 +122,12 @@ inline b3SparseSymMat33::b3SparseSymMat33(const b3SparseSymMat33& m) Copy(m); } -inline b3SparseSymMat33::~b3SparseSymMat33() +inline b3SparseMat33::~b3SparseMat33() { Destroy(); } -inline void b3SparseSymMat33::Destroy() +inline void b3SparseMat33::Destroy() { for (u32 i = 0; i < rowCount; ++i) { @@ -179,7 +147,7 @@ inline void b3SparseSymMat33::Destroy() b3Free(rows); } -inline b3SparseSymMat33& b3SparseSymMat33::operator=(const b3SparseSymMat33& _m) +inline b3SparseMat33& b3SparseMat33::operator=(const b3SparseMat33& _m) { if (_m.rows == rows) { @@ -200,7 +168,7 @@ inline b3SparseSymMat33& b3SparseSymMat33::operator=(const b3SparseSymMat33& _m) return *this; } -inline void b3SparseSymMat33::Copy(const b3SparseSymMat33& _m) +inline void b3SparseMat33::Copy(const b3SparseMat33& _m) { B3_ASSERT(rowCount == _m.rowCount); @@ -223,29 +191,16 @@ inline void b3SparseSymMat33::Copy(const b3SparseSymMat33& _m) } } -inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const +inline const b3Mat33& b3SparseMat33::operator()(u32 i, u32 j) const { B3_ASSERT(i < rowCount); B3_ASSERT(j < rowCount); - // Ensure i, and j is on the upper triangle - if (i > j) - { - b3Swap(i, j); - } - b3RowValueList* vs = rows + i; for (b3RowValue* v = vs->head; v; v = v->next) { - u32 column = v->column; - - if (column < j) - { - break; - } - - if (column == j) + if (v->column == j) { return v->value; } @@ -254,64 +209,31 @@ inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const return b3Mat33_zero; } -inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) -{ +inline b3Mat33& b3SparseMat33::operator()(u32 i, u32 j) +{ B3_ASSERT(i < rowCount); B3_ASSERT(j < rowCount); - // Ensure i, and j is on the upper triangle - if (i > j) - { - b3Swap(i, j); - } - b3RowValueList* vs = rows + i; for (b3RowValue* v = vs->head; v; v = v->next) { - u32 column = v->column; - - if (column < j) - { - break; - } - - if (column == j) + if (v->column == j) { return v->value; } } - b3RowValue* v1 = (b3RowValue*)b3Alloc(sizeof(b3RowValue)); - v1->column = j; - v1->value.SetZero(); + b3RowValue* v = (b3RowValue*)b3Alloc(sizeof(b3RowValue)); + v->column = j; + v->value.SetZero(); - b3RowValue* v0 = nullptr; + vs->PushFront(v); - for (b3RowValue* v = vs->head; v; v = v->next) - { - u32 column = v->column; - - if (column > j) - { - v0 = v; - break; - } - } - - if (v0 == nullptr) - { - vs->PushFront(v1); - } - else - { - vs->PushAfter(v0, v1); - } - - return v1->value; + return v->value; } -inline void b3SparseSymMat33::operator+=(const b3SparseSymMat33& m) +inline void b3SparseMat33::operator+=(const b3SparseMat33& m) { B3_ASSERT(rowCount == m.rowCount); @@ -328,7 +250,7 @@ inline void b3SparseSymMat33::operator+=(const b3SparseSymMat33& m) } } -inline void b3SparseSymMat33::operator-=(const b3SparseSymMat33& m) +inline void b3SparseMat33::operator-=(const b3SparseMat33& m) { B3_ASSERT(rowCount == m.rowCount); @@ -345,29 +267,19 @@ inline void b3SparseSymMat33::operator-=(const b3SparseSymMat33& m) } } -inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const -{ - B3_ASSERT(rowCount == out.n); - - for (u32 i = 0; i < rowCount; ++i) - { - out[i] = (*this)(i, i); - } -} - -inline void b3Add(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b) +inline void b3Add(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b) { out = a; out += b; } -inline void b3Sub(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b) +inline void b3Sub(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b) { out = a; out -= b; } -inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v) +inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v) { B3_ASSERT(A.rowCount == out.n); @@ -383,17 +295,11 @@ inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3 b3Mat33 a = vA->value; out[i] += a * v[j]; - - if (i != j) - { - // a_ij == a_ji - out[j] += a * v[i]; - } } } } -inline void b3Mul(b3SparseSymMat33& out, float32 s, const b3SparseSymMat33& B) +inline void b3Mul(b3SparseMat33& out, float32 s, const b3SparseMat33& B) { B3_ASSERT(out.rowCount == B.rowCount); @@ -402,42 +308,40 @@ inline void b3Mul(b3SparseSymMat33& out, float32 s, const b3SparseSymMat33& B) return; } - for (u32 i = 0; i < B.rowCount; ++i) + out = B; + + for (u32 i = 0; i < out.rowCount; ++i) { - b3RowValueList* vs = B.rows + i; - - for (b3RowValue* vB = vs->head; vB; vB = vB->next) + b3RowValueList* vs = out.rows + i; + for (b3RowValue* v = vs->head; v; v = v->next) { - u32 j = vB->column; - b3Mat33 b = vB->value; - - out(i, j) = s * b; + v->value = s * v->value; } } } -inline b3SparseSymMat33 operator+(const b3SparseSymMat33& A, const b3SparseSymMat33& B) +inline b3SparseMat33 operator+(const b3SparseMat33& A, const b3SparseMat33& B) { - b3SparseSymMat33 result(A.rowCount); + b3SparseMat33 result(A.rowCount); b3Add(result, A, B); return result; } -inline b3SparseSymMat33 operator-(const b3SparseSymMat33& A, const b3SparseSymMat33& B) +inline b3SparseMat33 operator-(const b3SparseMat33& A, const b3SparseMat33& B) { - b3SparseSymMat33 result(A.rowCount); + b3SparseMat33 result(A.rowCount); b3Sub(result, A, B); return result; } -inline b3SparseSymMat33 operator*(float32 A, const b3SparseSymMat33& B) +inline b3SparseMat33 operator*(float32 A, const b3SparseMat33& B) { - b3SparseSymMat33 result(B.rowCount); + b3SparseMat33 result(B.rowCount); b3Mul(result, A, B); return result; } -inline b3DenseVec3 operator*(const b3SparseSymMat33& A, const b3DenseVec3& v) +inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v) { b3DenseVec3 result(v.n); b3Mul(result, A, v); diff --git a/include/bounce/sparse/sparse_mat33_view.h b/include/bounce/sparse/sparse_mat33_view.h new file mode 100644 index 0000000..9d92520 --- /dev/null +++ b/include/bounce/sparse/sparse_mat33_view.h @@ -0,0 +1,132 @@ +/* +* 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_VIEW_H +#define B3_SPARSE_MAT_33_VIEW_H + +#include + +struct b3ArrayRowValue +{ + u32 column; + b3Mat33 value; +}; + +struct b3RowValueArray +{ + u32 count; + b3ArrayRowValue* values; +}; + +// A read-only sparse matrix. +struct b3SparseMat33View +{ + // + b3SparseMat33View(const b3SparseMat33& _m); + + // + ~b3SparseMat33View(); + + // + const b3Mat33& operator()(u32 i, u32 j) const; + + u32 rowCount; + b3RowValueArray* rows; +}; + +inline b3SparseMat33View::b3SparseMat33View(const b3SparseMat33& _m) +{ + rowCount = _m.rowCount; + rows = (b3RowValueArray*)b3Alloc(rowCount * sizeof(b3RowValueArray)); + for (u32 i = 0; i < _m.rowCount; ++i) + { + b3RowValueList* rowList = _m.rows + i; + b3RowValueArray* rowArray = rows + i; + + rowArray->count = rowList->count; + rowArray->values = (b3ArrayRowValue*)b3Alloc(rowArray->count * sizeof(b3ArrayRowValue)); + + u32 valueIndex = 0; + for (b3RowValue* v = rowList->head; v; v = v->next) + { + rowArray->values[valueIndex].column = v->column; + rowArray->values[valueIndex].value = v->value; + ++valueIndex; + } + } +} + +inline b3SparseMat33View::~b3SparseMat33View() +{ + for (u32 i = 0; i < rowCount; ++i) + { + b3RowValueArray* rowArray = rows + i; + b3Free(rowArray->values); + } + b3Free(rows); +} + +inline const b3Mat33& b3SparseMat33View::operator()(u32 i, u32 j) const +{ + B3_ASSERT(i < rowCount); + B3_ASSERT(j < rowCount); + + b3RowValueArray* vs = rows + i; + + for (u32 c = 0; c < vs->count; ++c) + { + b3ArrayRowValue* rv = vs->values + c; + if (rv->column == j) + { + return rv->value; + } + } + + return b3Mat33_zero; +} + +inline void b3Mul(b3DenseVec3& out, const b3SparseMat33View& A, const b3DenseVec3& v) +{ + B3_ASSERT(A.rowCount == out.n); + + out.SetZero(); + + for (u32 i = 0; i < A.rowCount; ++i) + { + b3RowValueArray* rowArray = A.rows + i; + + for (u32 c = 0; c < rowArray->count; ++c) + { + b3ArrayRowValue* rv = rowArray->values + c; + + u32 j = rv->column; + b3Mat33 a = rv->value; + + out[i] += a * v[j]; + } + } +} + +inline b3DenseVec3 operator*(const b3SparseMat33View& A, const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + b3Mul(result, A, v); + return result; +} + +#endif diff --git a/license.txt b/license.txt index a99dc41..afe08e9 100644 --- a/license.txt +++ b/license.txt @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -14,4 +14,4 @@ * 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. -*/ \ No newline at end of file +*/ diff --git a/premake5.lua b/premake5.lua index 3c43015..cc2c029 100644 --- a/premake5.lua +++ b/premake5.lua @@ -264,9 +264,8 @@ workspace(solution_name) { examples_inc_dir .. "/testbed/framework/draw.h", examples_inc_dir .. "/testbed/framework/profiler.h", - examples_inc_dir .. "/testbed/framework/recorder_profiler.h", + examples_inc_dir .. "/testbed/framework/profiler_st.h", examples_inc_dir .. "/testbed/framework/json_profiler.h", - examples_inc_dir .. "/testbed/framework/testbed_listener.h", examples_inc_dir .. "/testbed/framework/model.h", examples_inc_dir .. "/testbed/framework/view.h", @@ -274,11 +273,19 @@ workspace(solution_name) examples_src_dir .. "/testbed/framework/test.h", + 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", examples_src_dir .. "/testbed/framework/draw.cpp", examples_src_dir .. "/testbed/framework/profiler.cpp", - examples_src_dir .. "/testbed/framework/recorder_profiler.cpp", + examples_src_dir .. "/testbed/framework/profiler_st.cpp", examples_src_dir .. "/testbed/framework/json_profiler.cpp", examples_inc_dir .. "/testbed/framework/model.cpp", @@ -365,7 +372,7 @@ end -- clean newaction { - trigger = "clean", + trigger = "clean", description = "Clean solution", execute = function () os.rmdir( "doc" ) diff --git a/readme.md b/readme.md index 8879dfa..c90f4ab 100644 --- a/readme.md +++ b/readme.md @@ -1,6 +1,6 @@ ## Bounce -**Welcome! Bounce is a 3D physics engine.** +**Welcome! Bounce is a 3D physics engine for games.** Feel free to ask questions, give feedback and suggestions using the issue tracker. @@ -20,7 +20,7 @@ Bounce uses [premake](https://premake.github.io/) for generating project files i * Say { premake5 vs2017 } on a command line. * Open build/vs2017/bounce.sln. * Set testbed as the startup project. -* In the testbed debugging properties, set the Working Directory to ..\..\examples\testbed. +* In the testbed debugging properties, set the Working Directory to ..\\..\examples\testbed. * Press F5 to run. ### Linux @@ -104,7 +104,6 @@ Below are the external dependencies for testbed. If you don't care about testbed ### Dynamics * Rigid bodies -* Cloth * Contact, friction, restitution * Mouse, spring, sphere, cone, revolute joint types * Quaternion constraints @@ -118,6 +117,29 @@ Below are the external dependencies for testbed. If you don't care about testbed * Contact callbacks: begin, pre-solve, post-solve * Ray-casting and volume queries +### Rope + +* Rope +* Linear time solver + +### Cloth + +* Cloth +* Vertex contact, friction +* Strech, shear, spring, mouse force types +* Linear time solver +* Unconditional simulation stability +* Ray-casting + +### Soft Body + +* Soft body +* Vertex contact, friction +* Elasticity, plasticity +* Linear time solver +* Unconditional simulation stability +* Ray-casting + ### Testbed * OpenGL with GLFW and GLAD diff --git a/screenshots/a.png b/screenshots/a.png deleted file mode 100644 index fe54a2a..0000000 Binary files a/screenshots/a.png and /dev/null differ diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp new file mode 100644 index 0000000..b5f957b --- /dev/null +++ b/src/bounce/cloth/cloth.cpp @@ -0,0 +1,675 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static B3_FORCE_INLINE u32 b3NextIndex(u32 i) +{ + return i + 1 < 3 ? i + 1 : 0; +} + +struct b3SharedEdge +{ + u32 v1, v2; + u32 nsv1, nsv2; +}; + +static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) +{ + u32 sharedCount = 0; + + for (u32 i = 0; i < m->triangleCount; ++i) + { + b3ClothMeshTriangle* t1 = m->triangles + i; + u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; + + for (u32 j1 = 0; j1 < 3; ++j1) + { + u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0; + + u32 t1v1 = i1s[j1]; + u32 t1v2 = i1s[k1]; + + for (u32 j = i + 1; j < m->triangleCount; ++j) + { + b3ClothMeshTriangle* t2 = m->triangles + j; + u32 i2s[3] = { t2->v1, t2->v2, t2->v3 }; + + for (u32 j2 = 0; j2 < 3; ++j2) + { + u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0; + + u32 t2v1 = i2s[j2]; + u32 t2v2 = i2s[k2]; + + if (t1v1 == t2v2 && t1v2 == t2v1) + { + // The triangles are adjacent. + u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0; + u32 t1v3 = i1s[k3]; + + u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0; + u32 t2v3 = i2s[k4]; + + // Add shared edge and non-shared vertices. + b3SharedEdge se; + se.v1 = t1v1; + se.v2 = t1v2; + se.nsv1 = t1v3; + se.nsv2 = t2v3; + + sharedEdges[sharedCount++] = se; + + break; + } + } + } + } + } + + return sharedCount; +} + +b3Cloth::b3Cloth(const b3ClothDef& def) : + m_particleBlocks(sizeof(b3Particle)) +{ + B3_ASSERT(def.mesh); + B3_ASSERT(def.density > 0.0f); + + m_mesh = def.mesh; + m_density = def.density; + m_contactManager.m_cloth = this; + + const b3ClothMesh* m = m_mesh; + + // Initialize particles + m_particles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); + for (u32 i = 0; i < m->vertexCount; ++i) + { + b3ParticleDef pd; + pd.type = e_dynamicParticle; + pd.mass = 1.0f; + pd.radius = def.thickness; + pd.friction = def.friction; + pd.position = m->vertices[i]; + + b3Particle* p = CreateParticle(pd); + + p->m_vertex = i; + m_particles[i] = p; + } + + // Compute mass + ComputeMass(); + + // Initialize triangles + m_triangles = (b3ClothTriangle*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothTriangle)); + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* meshTriangle = m_mesh->triangles + i; + b3ClothTriangle* triangle = m_triangles + i; + + triangle->m_cloth = this; + triangle->m_radius = def.thickness; + triangle->m_friction = def.friction; + triangle->m_triangle = i; + + b3Vec3 A = m_mesh->vertices[meshTriangle->v1]; + b3Vec3 B = m_mesh->vertices[meshTriangle->v2]; + b3Vec3 C = m_mesh->vertices[meshTriangle->v3]; + + b3AABB3 aabb; + aabb.Set(A, B, C); + aabb.Extend(triangle->m_radius); + + triangle->m_aabbProxy.type = e_triangleProxy; + triangle->m_aabbProxy.owner = triangle; + triangle->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &triangle->m_aabbProxy); + + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + // uv1 + float32 u1 = 0.0f; + float32 v1 = 0.0f; + + // uv2 + float32 u2 = b3Length(AB); + float32 v2 = 0.0f; + + // uv3 + B3_ASSERT(u2 > 0.0f); + b3Vec3 n_AB = AB / u2; + + // A = b * h / 2 + // h = (A * 2) / b + float32 A2 = b3Length(b3Cross(AB, AC)); + B3_ASSERT(A2 > 0.0f); + + float32 u3 = b3Dot(AC, n_AB); + float32 v3 = A2 / u2; + + // Strech matrix + float32 du1 = u2 - u1; + float32 dv1 = v2 - v1; + float32 du2 = u3 - u1; + float32 dv2 = v3 - v1; + + triangle->m_du1 = du1; + triangle->m_dv1 = dv1; + triangle->m_du2 = du2; + triangle->m_dv2 = dv2; + + float32 det = du1 * dv2 - du2 * dv1; + B3_ASSERT(det != 0.0f); + triangle->m_inv_det = 1.0f / det; + + // Area + triangle->m_alpha = 0.5f * A2; + + // Create strech force + b3StrechForceDef sfdef; + sfdef.triangle = triangle; + sfdef.streching = def.streching; + sfdef.damping = def.damping; + sfdef.bu = 1.0f; + sfdef.bv = 1.0f; + + if (def.streching > 0.0f) + { + CreateForce(sfdef); + } + + b3ShearForceDef shdef; + shdef.triangle = triangle; + shdef.shearing = def.shearing; + shdef.damping = def.damping; + + if (def.shearing > 0.0f) + { + CreateForce(shdef); + } + } + + // Initialize forces + b3StackAllocator* allocator = &m_stackAllocator; + + // Worst-case edge memory + u32 edgeCount = 3 * m->triangleCount; + + b3SharedEdge* sharedEdges = (b3SharedEdge*)allocator->Allocate(edgeCount * sizeof(b3SharedEdge)); + u32 sharedCount = b3FindSharedEdges(sharedEdges, m); + + // Bending + for (u32 i = 0; i < sharedCount; ++i) + { + b3SharedEdge* e = sharedEdges + i; + + b3Particle* p1 = m_particles[e->v1]; + b3Particle* p2 = m_particles[e->v2]; + b3Particle* p3 = m_particles[e->nsv1]; + b3Particle* p4 = m_particles[e->nsv2]; + + b3SpringForceDef fd; + fd.Initialize(p3, p4, def.bending, def.damping); + + if (def.bending > 0.0f) + { + CreateForce(fd); + } + } + + allocator->Free(sharedEdges); + + // Sewing + for (u32 i = 0; i < m->sewingLineCount; ++i) + { + b3ClothMeshSewingLine* line = m->sewingLines + i; + + b3Particle* p1 = m_particles[line->v1]; + b3Particle* p2 = m_particles[line->v2]; + + b3SpringForceDef fd; + fd.Initialize(p1, p2, def.sewing, def.damping); + + if (def.sewing > 0.0f) + { + CreateForce(fd); + } + } + + m_gravity.SetZero(); + m_world = nullptr; +} + +b3Cloth::~b3Cloth() +{ + b3Free(m_particles); + b3Free(m_triangles); + + b3Particle* p = m_particleList.m_head; + while (p) + { + b3Particle* p0 = p; + p = p->m_next; + p0->~b3Particle(); + } + + b3Force* f = m_forceList.m_head; + while (f) + { + b3Force* f0 = f; + f = f->m_next; + b3Force::Destroy(f0); + } +} + +void b3Cloth::SetWorld(b3World* world) +{ + if (!world && m_world) + { + // Destroy body contacts + b3ParticleBodyContact* c = m_contactManager.m_particleBodyContactList.m_head; + while (c) + { + b3ParticleBodyContact* boom = c; + c = c->m_next; + m_contactManager.Destroy(boom); + } + } + + m_world = world; +} + +b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def) +{ + void* mem = m_particleBlocks.Allocate(); + b3Particle* p = new(mem) b3Particle(def, this); + + b3AABB3 aabb; + aabb.Set(p->m_position, p->m_radius); + + p->m_aabbProxy.type = e_particleProxy; + p->m_aabbProxy.owner = p; + p->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &p->m_aabbProxy); + + m_particleList.PushFront(p); + + return p; +} + +void b3Cloth::DestroyParticle(b3Particle* particle) +{ + B3_ASSERT(particle->m_vertex == ~0); + + // Destroy particle forces + b3Force* f = m_forceList.m_head; + while (f) + { + b3Force* f0 = f; + f = f->m_next; + + if (f0->HasParticle(particle)) + { + m_forceList.Remove(f0); + b3Force::Destroy(f0); + } + } + + // Destroy particle contacts + particle->DestroyContacts(); + + // Destroy AABB proxy + m_contactManager.m_broadPhase.DestroyProxy(particle->m_broadPhaseId); + + m_particleList.Remove(particle); + particle->~b3Particle(); + m_particleBlocks.Free(particle); +} + +b3Force* b3Cloth::CreateForce(const b3ForceDef& def) +{ + b3Force* f = b3Force::Create(&def); + m_forceList.PushFront(f); + return f; +} + +void b3Cloth::DestroyForce(b3Force* force) +{ + m_forceList.Remove(force); + b3Force::Destroy(force); +} + +float32 b3Cloth::GetEnergy() const +{ + float32 E = 0.0f; + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + E += p->m_mass * b3Dot(p->m_velocity, p->m_velocity); + } + return 0.5f * E; +} + +b3Particle* b3Cloth::GetParticle(u32 i) +{ + B3_ASSERT(i < m_mesh->vertexCount); + return m_particles[i]; +} + +b3ClothTriangle* b3Cloth::GetTriangle(u32 i) +{ + B3_ASSERT(i < m_mesh->triangleCount); + return m_triangles + i; +} + +void b3Cloth::ComputeMass() +{ + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + p->m_mass = 0.0f; + p->m_invMass = 0.0f; + } + + const float32 inv3 = 1.0f / 3.0f; + const float32 rho = m_density; + + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* triangle = m_mesh->triangles + i; + + b3Vec3 v1 = m_mesh->vertices[triangle->v1]; + b3Vec3 v2 = m_mesh->vertices[triangle->v2]; + b3Vec3 v3 = m_mesh->vertices[triangle->v3]; + + float32 area = b3Area(v1, v2, v3); + B3_ASSERT(area > 0.0f); + + float32 mass = rho * area; + + b3Particle* p1 = m_particles[triangle->v1]; + b3Particle* p2 = m_particles[triangle->v2]; + b3Particle* p3 = m_particles[triangle->v3]; + + p1->m_mass += inv3 * mass; + p2->m_mass += inv3 * mass; + p3->m_mass += inv3 * mass; + } + + // Invert + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + B3_ASSERT(p->m_mass > 0.0f); + p->m_invMass = 1.0f / p->m_mass; + } +} + +struct b3ClothRayCastSingleCallback +{ + float32 Report(const b3RayCastInput& input, u32 proxyId) + { + // Get primitive associated with the proxy. + void* userData = broadPhase->GetUserData(proxyId); + b3ClothAABBProxy* proxy = (b3ClothAABBProxy*)userData; + + if (proxy->type != e_triangleProxy) + { + // Continue search from where we stopped. + return input.maxFraction; + } + + b3ClothTriangle* triangle = (b3ClothTriangle*)proxy->owner; + u32 triangleIndex = triangle->GetTriangle(); + + b3RayCastOutput subOutput; + if (cloth->RayCast(&subOutput, &input, triangleIndex)) + { + // Ray hits triangle. + if (subOutput.fraction < output0.fraction) + { + triangle0 = triangleIndex; + output0.fraction = subOutput.fraction; + output0.normal = subOutput.normal; + } + } + + // Continue search from where we stopped. + return input.maxFraction; + } + + const b3Cloth* cloth; + const b3BroadPhase* broadPhase; + u32 triangle0; + b3RayCastOutput output0; +}; + +bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const +{ + b3RayCastInput input; + input.p1 = p1; + input.p2 = p2; + input.maxFraction = 1.0f; + + b3ClothRayCastSingleCallback callback; + callback.cloth = this; + callback.broadPhase = &m_contactManager.m_broadPhase; + callback.triangle0 = ~0; + callback.output0.fraction = B3_MAX_FLOAT; + + m_contactManager.m_broadPhase.RayCast(&callback, input); + + if (callback.triangle0 != ~0) + { + output->triangle = callback.triangle0; + output->fraction = callback.output0.fraction; + output->normal = callback.output0.normal; + + return true; + } + + return false; +} + +bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const +{ + B3_ASSERT(triangleIndex < m_mesh->triangleCount); + b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex; + + b3Vec3 v1 = m_particles[triangle->v1]->m_position; + b3Vec3 v2 = m_particles[triangle->v2]->m_position; + b3Vec3 v3 = m_particles[triangle->v3]->m_position; + + return b3RayCast(output, input, v1, v2, v3); +} + +void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) +{ + B3_PROFILE("Cloth Solve"); + + // Solve + b3ClothSolverDef solverDef; + solverDef.stack = &m_stackAllocator; + solverDef.particleCapacity = m_particleList.m_count; + solverDef.forceCapacity = m_forceList.m_count; + solverDef.bodyContactCapacity = m_contactManager.m_particleBodyContactList.m_count; + solverDef.triangleContactCapacity = m_contactManager.m_particleTriangleContactList.m_count; + + b3ClothSolver solver(solverDef); + + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + solver.Add(p); + } + + for (b3Force* f = m_forceList.m_head; f; f = f->m_next) + { + solver.Add(f); + } + + for (b3ParticleTriangleContact* c = m_contactManager.m_particleTriangleContactList.m_head; c; c = c->m_next) + { + if (c->m_active) + { + solver.Add(c); + } + } + + for (b3ParticleBodyContact* c = m_contactManager.m_particleBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_active) + { + solver.Add(c); + } + } + + // Solve + solver.Solve(dt, gravity, velocityIterations, positionIterations); +} + +void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) +{ + B3_PROFILE("Cloth Step"); + + // Update contacts + m_contactManager.UpdateContacts(); + + // Integrate state, solve constraints. + if (dt > 0.0f) + { + Solve(dt, m_gravity, velocityIterations, positionIterations); + } + + // Clear external applied forces and translations + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + p->m_force.SetZero(); + p->m_translation.SetZero(); + } + + // Synchronize particles + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + b3Vec3 displacement = dt * p->m_velocity; + + p->Synchronize(displacement); + } + + // Synchronize triangles + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* triangle = m_mesh->triangles + i; + + b3Particle* p1 = m_particles[triangle->v1]; + b3Particle* p2 = m_particles[triangle->v2]; + b3Particle* p3 = m_particles[triangle->v3]; + + b3Vec3 v1 = p1->m_velocity; + b3Vec3 v2 = p2->m_velocity; + b3Vec3 v3 = p3->m_velocity; + + b3Vec3 velocity = (v1 + v2 + v3) / 3.0f; + + b3Vec3 displacement = dt * velocity; + + m_triangles[i].Synchronize(displacement); + } + + // Find new contacts + m_contactManager.FindNewContacts(); +} + +void b3Cloth::Draw() const +{ + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + if (p->m_type == e_staticParticle) + { + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_white); + } + + if (p->m_type == e_kinematicParticle) + { + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_blue); + } + + if (p->m_type == e_dynamicParticle) + { + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green); + } + } + + for (b3Force* f = m_forceList.m_head; f; f = f->m_next) + { + if (f->m_type == e_springForce) + { + b3SpringForce* s = (b3SpringForce*)f; + b3Particle* p1 = s->m_p1; + b3Particle* p2 = s->m_p2; + + b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_black); + } + } + + const b3ClothMesh* m = m_mesh; + + for (u32 i = 0; i < m->sewingLineCount; ++i) + { + b3ClothMeshSewingLine* s = m->sewingLines + i; + b3Particle* p1 = m_particles[s->v1]; + b3Particle* p2 = m_particles[s->v2]; + + b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_white); + } + + for (u32 i = 0; i < m->triangleCount; ++i) + { + b3ClothMeshTriangle* t = m->triangles + i; + + b3Particle* p1 = m_particles[t->v1]; + b3Particle* p2 = m_particles[t->v2]; + b3Particle* p3 = m_particles[t->v3]; + + b3Vec3 v1 = p1->m_position; + b3Vec3 v2 = p2->m_position; + b3Vec3 v3 = p3->m_position; + + b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black); + + b3Vec3 c = (v1 + v2 + v3) / 3.0f; + + float32 s = 0.9f; + + v1 = s * (v1 - c) + c; + v2 = s * (v2 - c) + c; + v3 = s * (v3 - c) + c; + + b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); + n1.Normalize(); + b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue); + + b3Vec3 n2 = -n1; + b3Draw_draw->DrawSolidTriangle(n2, v3, v2, v1, b3Color_blue); + } +} \ No newline at end of file diff --git a/src/bounce/cloth/cloth_contact_manager.cpp b/src/bounce/cloth/cloth_contact_manager.cpp new file mode 100644 index 0000000..77c09bd --- /dev/null +++ b/src/bounce/cloth/cloth_contact_manager.cpp @@ -0,0 +1,315 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +b3ClothContactManager::b3ClothContactManager() : + m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact)), + m_particleBodyContactBlocks(sizeof(b3ParticleBodyContact)) +{ + +} + +void b3ClothContactManager::FindNewContacts() +{ + FindNewClothContacts(); + FindNewBodyContacts(); +} + +void b3ClothContactManager::FindNewClothContacts() +{ + B3_PROFILE("Cloth Find New Cloth Contacts"); + + m_broadPhase.FindPairs(this); +} + +class b3ClothContactManagerFindNewBodyContactsQueryListener : public b3QueryListener +{ +public: + virtual bool ReportShape(b3Shape* s2) + { + cm->AddPSPair(p1, s2); + + // Keep looking for overlaps + return true; + } + + b3ClothContactManager* cm; + b3Particle* p1; +}; + +void b3ClothContactManager::FindNewBodyContacts() +{ + B3_PROFILE("Cloth Find New Body Contacts"); + + // Is there a world attached to this cloth? + if (m_cloth->m_world == nullptr) + { + return; + } + + for (b3Particle* p = m_cloth->m_particleList.m_head; p; p = p->m_next) + { + if (p->m_type != e_dynamicParticle) + { + continue; + } + + b3AABB3 aabb = m_broadPhase.GetAABB(p->m_broadPhaseId); + + b3ClothContactManagerFindNewBodyContactsQueryListener listener; + listener.cm = this; + listener.p1 = p; + + m_cloth->m_world->QueryAABB(&listener, aabb); + } +} + +void b3ClothContactManager::AddPSPair(b3Particle* p1, b3Shape* s2) +{ + // Check if there is a contact between the two entities. + for (b3ParticleBodyContact* c = m_particleBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_p1 == p1 && c->m_s2 == s2) + { + // A contact already exists. + return; + } + } + + bool isntDynamic1 = p1->m_type != e_dynamicParticle; + bool isntDynamic2 = s2->GetBody()->GetType() != e_dynamicBody; + + if (isntDynamic1 && isntDynamic2) + { + // The entities must not collide with each other. + return; + } + + // Create a new contact. + b3ParticleBodyContact* c = CreateParticleBodyContact(); + + c->m_p1 = p1; + c->m_s2 = s2; + c->m_active = false; + c->m_normalImpulse = 0.0f; + c->m_tangentImpulse.SetZero(); + + // Add the contact to the body contact list. + m_particleBodyContactList.PushFront(c); +} + +void b3ClothContactManager::AddPair(void* data1, void* data2) +{ + b3ClothAABBProxy* proxy1 = (b3ClothAABBProxy*)data1; + b3ClothAABBProxy* proxy2 = (b3ClothAABBProxy*)data2; + + if (proxy1->type == e_particleProxy && proxy2->type == e_particleProxy) + { + // Particle-particle contacts are not supported. + return; + } + + if (proxy1->type == e_triangleProxy && proxy2->type == e_triangleProxy) + { + // Triangle-triangle contacts are not supported. + return; + } + + if (proxy1->type == e_triangleProxy) + { + // Ensure proxy1 is a particle and proxy 2 a triangle. + b3Swap(proxy1, proxy2); + } + + B3_ASSERT(proxy1->type == e_particleProxy); + B3_ASSERT(proxy2->type == e_triangleProxy); + + b3Particle* p1 = (b3Particle*)proxy1->owner; + + b3ClothTriangle* t2 = (b3ClothTriangle*)proxy2->owner; + b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + t2->m_triangle; + b3Particle* p2 = m_cloth->m_particles[triangle->v1]; + b3Particle* p3 = m_cloth->m_particles[triangle->v2]; + b3Particle* p4 = m_cloth->m_particles[triangle->v3]; + + // Check if there is a contact between the two entities. + for (b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; c; c = c->m_next) + { + if (c->m_p1 == p1 && c->m_t2 == t2) + { + // A contact already exists. + return; + } + } + + bool isntDynamic1 = p1->m_type != e_dynamicParticle; + bool isntDynamic2 = p2->m_type != e_dynamicParticle && p3->m_type != e_dynamicParticle && p4->m_type != e_dynamicParticle; + + if (isntDynamic1 && isntDynamic2) + { + // The entities must not collide with each other. + return; + } + + if (p1 == p2 || p1 == p3 || p1 == p4) + { + // The entities must not collide with each other. + return; + } + + // Create a new contact. + b3ParticleTriangleContact* c = CreateParticleTriangleContact(); + + c->m_p1 = p1; + c->m_t2 = t2; + c->m_p2 = p2; + c->m_p3 = p3; + c->m_p4 = p4; + c->m_normalImpulse = 0.0f; + c->m_tangentImpulse1 = 0.0f; + c->m_tangentImpulse2 = 0.0f; + c->m_active = false; + + // Add the contact to the cloth contact list. + m_particleTriangleContactList.PushFront(c); +} + +b3ParticleTriangleContact* b3ClothContactManager::CreateParticleTriangleContact() +{ + void* block = m_particleTriangleContactBlocks.Allocate(); + return new(block) b3ParticleTriangleContact(); +} + +void b3ClothContactManager::Destroy(b3ParticleTriangleContact* c) +{ + m_particleTriangleContactList.Remove(c); + + c->~b3ParticleTriangleContact(); + + m_particleTriangleContactBlocks.Free(c); +} + +b3ParticleBodyContact* b3ClothContactManager::CreateParticleBodyContact() +{ + void* block = m_particleBodyContactBlocks.Allocate(); + return new(block) b3ParticleBodyContact(); +} + +void b3ClothContactManager::Destroy(b3ParticleBodyContact* c) +{ + m_particleBodyContactList.Remove(c); + + c->~b3ParticleBodyContact(); + + m_particleBodyContactBlocks.Free(c); +} + +void b3ClothContactManager::UpdateContacts() +{ + UpdateClothContacts(); + UpdateBodyContacts(); +} + +void b3ClothContactManager::UpdateClothContacts() +{ + B3_PROFILE("Cloth Update Cloth Contacts"); + + // Update the state of particle-triangle contacts. + b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; + while (c) + { + bool isntDynamic1 = c->m_p1->m_type != e_dynamicParticle; + bool isntDynamic2 = c->m_p2->m_type != e_dynamicParticle && c->m_p3->m_type != e_dynamicParticle && c->m_p4->m_type != e_dynamicParticle; + + // Destroy the contact if primitives must not collide with each other. + if (isntDynamic1 && isntDynamic2) + { + b3ParticleTriangleContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + u32 proxy1 = c->m_p1->m_broadPhaseId; + u32 proxy2 = c->m_t2->m_broadPhaseId; + + // Destroy the contact if primitive AABBs are not overlapping. + bool overlap = m_broadPhase.TestOverlap(proxy1, proxy2); + if (overlap == false) + { + b3ParticleTriangleContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + // The contact persists. + c->Update(); + + c = c->m_next; + } +} + +void b3ClothContactManager::UpdateBodyContacts() +{ + B3_PROFILE("Cloth Update Body Contacts"); + + // Update the state of particle-body contacts. + b3ParticleBodyContact* c = m_particleBodyContactList.m_head; + while (c) + { + bool isntDynamic1 = c->m_p1->m_type != e_dynamicParticle; + bool isntDynamic2 = c->m_s2->GetBody()->GetType() != e_dynamicBody; + + // Cease the contact if entities must not collide with each other. + if (isntDynamic1 && isntDynamic2) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + b3AABB3 aabb1 = m_broadPhase.GetAABB(c->m_p1->m_broadPhaseId); + b3AABB3 aabb2 = c->m_s2->GetAABB(); + + // Destroy the contact if entities AABBs are not overlapping. + bool overlap = b3TestOverlap(aabb1, aabb2); + if (overlap == false) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + // The contact persists. + c->Update(); + + c = c->m_next; + } +} \ No newline at end of file diff --git a/src/bounce/cloth/cloth_force_solver.cpp b/src/bounce/cloth/cloth_force_solver.cpp new file mode 100644 index 0000000..f801174 --- /dev/null +++ b/src/bounce/cloth/cloth_force_solver.cpp @@ -0,0 +1,223 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. +// described in the paper: +// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". + +// Some improvements for the original MPCG algorithm are described in the paper: +// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". + +u32 b3_clothSolverIterations = 0; + +b3ClothForceSolver::b3ClothForceSolver(const b3ClothForceSolverDef& def) +{ + m_allocator = def.stack; + + m_particleCount = def.particleCount; + m_particles = def.particles; + + m_forceCount = def.forceCount; + m_forces = def.forces; +} + +b3ClothForceSolver::~b3ClothForceSolver() +{ +} + +void b3ClothForceSolver::ApplyForces() +{ + for (u32 i = 0; i < m_forceCount; ++i) + { + m_forces[i]->Apply(&m_solverData); + } +} + +// Solve Ax = b +static void b3SolveMPCG(b3DenseVec3& x, + const b3SparseMat33View& A, const b3DenseVec3& b, + const b3DiagMat33& S, const b3DenseVec3& z, + const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20) +{ + B3_PROFILE("Cloth Solve MPCG"); + + // Jacobi preconditioner + // P = diag(A) + b3DiagMat33 inv_P(A.rowCount); + for (u32 i = 0; i < A.rowCount; ++i) + { + b3Mat33 a = A(i, i); + + // Sylvester Criterion to ensure PD-ness + B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f); + + B3_ASSERT(a.x.x > 0.0f); + float32 xx = 1.0f / a.x.x; + + B3_ASSERT(a.y.y > 0.0f); + float32 yy = 1.0f / a.y.y; + + B3_ASSERT(a.z.z > 0.0f); + float32 zz = 1.0f / a.z.z; + + inv_P[i] = b3Diagonal(xx, yy, zz); + } + + x = (S * y) + (I - S) * z; + + b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); + + float32 b_delta = b3Dot(b_hat, inv_P * b_hat); + + b3DenseVec3 r = S * (b - A * x); + + b3DenseVec3 p = S * (inv_P * r); + + float32 delta_new = b3Dot(r, p); + + u32 iteration = 0; + for (;;) + { + if (iteration == maxIterations) + { + break; + } + + if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta) + { + break; + } + + b3DenseVec3 s = S * (A * p); + + float32 alpha = delta_new / b3Dot(p, s); + + x = x + alpha * p; + r = r - alpha * s; + + b3DenseVec3 h = inv_P * r; + + float32 delta_old = delta_new; + + delta_new = b3Dot(r, h); + + float32 beta = delta_new / delta_old; + + p = S * (h + beta * p); + + ++iteration; + } + + b3_clothSolverIterations = iteration; +} + +void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity) +{ + float32 h = dt; + + b3DenseVec3 sx(m_particleCount); + b3DenseVec3 sv(m_particleCount); + b3DenseVec3 sf(m_particleCount); + b3DenseVec3 sy(m_particleCount); + b3DenseVec3 sz(m_particleCount); + b3DenseVec3 sx0(m_particleCount); + b3SparseMat33 M(m_particleCount); + b3SparseMat33 dfdx(m_particleCount); + b3SparseMat33 dfdv(m_particleCount); + b3DiagMat33 S(m_particleCount); + b3DiagMat33 I(m_particleCount); + I.SetIdentity(); + + m_solverData.x = &sx; + m_solverData.v = &sv; + m_solverData.f = &sf; + m_solverData.y = &sy; + m_solverData.z = &sz; + m_solverData.dfdx = &dfdx; + m_solverData.dfdv = &dfdv; + m_solverData.S = &S; + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + M(i, i) = b3Diagonal(p->m_mass); + + sx[i] = p->m_position; + sv[i] = p->m_velocity; + sf[i] = p->m_force; + sz[i].SetZero(); + + if (p->m_type == e_dynamicParticle) + { + // Apply weight + sf[i] += p->m_mass * gravity; + S[i].SetIdentity(); + } + else + { + S[i].SetZero(); + } + + sy[i] = p->m_translation; + sx0[i] = p->m_x; + } + + // Apply internal forces + ApplyForces(); + + // Solve Ax = b, where + // A = M - h * dfdv - h * h * dfdx + // b = h * (f0 + h * dfdx * v0 + dfdx * y) + + // A + b3SparseMat33 A = M - h * dfdv - (h * h) * dfdx; + + // View for A + b3SparseMat33View viewA(A); + + // b + b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); + + // x + b3DenseVec3 x(m_particleCount); + b3SolveMPCG(x, viewA, b, S, sz, sx0, I); + + // Velocity update + sv = sv + x; + sx = sx + sy; + + // Copy state buffers back to the particle + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + p->m_x = x[i]; + p->m_position = sx[i]; + p->m_velocity = sv[i]; + } +} \ No newline at end of file diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp new file mode 100644 index 0000000..44203a2 --- /dev/null +++ b/src/bounce/cloth/cloth_solver.cpp @@ -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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) +{ + m_allocator = def.stack; + + m_particleCapacity = def.particleCapacity; + m_particleCount = 0; + m_particles = (b3Particle**)m_allocator->Allocate(m_particleCapacity * sizeof(b3Particle*)); + + m_forceCapacity = def.forceCapacity; + m_forceCount = 0; + m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));; + + m_bodyContactCapacity = def.bodyContactCapacity; + m_bodyContactCount = 0; + m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));; + + m_triangleContactCapacity = def.triangleContactCapacity; + m_triangleContactCount = 0; + m_triangleContacts = (b3ParticleTriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ParticleTriangleContact*));; +} + +b3ClothSolver::~b3ClothSolver() +{ + m_allocator->Free(m_triangleContacts); + m_allocator->Free(m_bodyContacts); + m_allocator->Free(m_forces); + m_allocator->Free(m_particles); +} + +void b3ClothSolver::Add(b3Particle* p) +{ + p->m_solverId = m_particleCount; + m_particles[m_particleCount++] = p; +} + +void b3ClothSolver::Add(b3Force* f) +{ + m_forces[m_forceCount++] = f; +} + +void b3ClothSolver::Add(b3ParticleBodyContact* c) +{ + m_bodyContacts[m_bodyContactCount++] = c; +} + +void b3ClothSolver::Add(b3ParticleTriangleContact* c) +{ + m_triangleContacts[m_triangleContactCount++] = c; +} + +void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) +{ + { + // Solve internal dynamics + b3ClothForceSolverDef forceSolverDef; + forceSolverDef.stack = m_allocator; + forceSolverDef.particleCount = m_particleCount; + forceSolverDef.particles = m_particles; + forceSolverDef.forceCount = m_forceCount; + forceSolverDef.forces = m_forces; + + b3ClothForceSolver forceSolver(forceSolverDef); + + forceSolver.Solve(dt, gravity); + } + + // Copy particle state to state buffer + b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); + b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); + for (u32 i = 0; i < m_particleCount; ++i) + { + positions[i] = m_particles[i]->m_position; + velocities[i] = m_particles[i]->m_velocity; + } + + { + // Solve constraints + b3ClothContactSolverDef contactSolverDef; + contactSolverDef.allocator = m_allocator; + contactSolverDef.positions = positions; + contactSolverDef.velocities = velocities; + contactSolverDef.bodyContactCount = m_bodyContactCount; + contactSolverDef.bodyContacts = m_bodyContacts; + contactSolverDef.triangleContactCount = m_triangleContactCount; + contactSolverDef.triangleContacts = m_triangleContacts; + + b3ClothContactSolver contactSolver(contactSolverDef); + + { + // Initialize constraints + contactSolver.InitializeBodyContactConstraints(); + contactSolver.InitializeTriangleContactConstraints(); + } + + { + // Warm start velocity constraints + contactSolver.WarmStartBodyContactConstraints(); + contactSolver.WarmStartTriangleContactConstraints(); + } + + { + // Solve velocity constraints + for (u32 i = 0; i < velocityIterations; ++i) + { + contactSolver.SolveBodyContactVelocityConstraints(); + contactSolver.SolveTriangleContactVelocityConstraints(); + } + } + + { + // Cache impulses for warm-starting + contactSolver.StoreImpulses(); + } + + // Integrate positions + float32 h = dt; + for (u32 i = 0; i < m_particleCount; ++i) + { + positions[i] += h * velocities[i]; + } + + { + // Solve position constraints + bool positionSolved = false; + for (u32 i = 0; i < positionIterations; ++i) + { + bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); + bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints(); + + if (bodyContactsSolved && triangleContactsSolved) + { + // Early out if the position errors are small. + positionSolved = true; + break; + } + } + } + + // Synchronize bodies + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3Body* body = m_bodyContacts[i]->m_s2->GetBody(); + + if (body->GetType() == e_staticBody) + { + continue; + } + + body->SynchronizeTransform(); + + body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation); + + body->SynchronizeShapes(); + } + } + + // Copy state buffers back to the particles + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i]->m_position = positions[i]; + m_particles[i]->m_velocity = velocities[i]; + } + + m_allocator->Free(velocities); + m_allocator->Free(positions); +} \ No newline at end of file diff --git a/src/bounce/cloth/cloth_triangle.cpp b/src/bounce/cloth/cloth_triangle.cpp new file mode 100644 index 0000000..bd5771a --- /dev/null +++ b/src/bounce/cloth/cloth_triangle.cpp @@ -0,0 +1,41 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +void b3ClothTriangle::Synchronize(const b3Vec3& displacement) +{ + b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + m_triangle; + + b3Particle* p1 = m_cloth->m_particles[triangle->v1]; + b3Particle* p2 = m_cloth->m_particles[triangle->v2]; + b3Particle* p3 = m_cloth->m_particles[triangle->v3]; + + b3Vec3 x1 = p1->m_position; + b3Vec3 x2 = p2->m_position; + b3Vec3 x3 = p3->m_position; + + b3AABB3 aabb; + aabb.Set(x1, x2, x3); + aabb.Extend(m_radius); + + m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_broadPhaseId, aabb, displacement); +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/contacts/cloth_contact_solver.cpp similarity index 52% rename from src/bounce/dynamics/cloth/cloth_contact_solver.cpp rename to src/bounce/cloth/contacts/cloth_contact_solver.cpp index cf05921..ae715c6 100644 --- a/src/bounce/dynamics/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/contacts/cloth_contact_solver.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,12 +16,13 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include +#include +#include +#include +#include +#include #include #include -#include b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def) { @@ -35,11 +36,6 @@ b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def) m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactVelocityConstraint)); m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactPositionConstraint)); - m_particleContactCount = def.particleContactCount; - m_particleContacts = def.particleContacts; - m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactVelocityConstraint)); - m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactPositionConstraint)); - m_triangleContactCount = def.triangleContactCount; m_triangleContacts = def.triangleContacts; m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactVelocityConstraint)); @@ -51,57 +47,52 @@ b3ClothContactSolver::~b3ClothContactSolver() m_allocator->Free(m_trianglePositionConstraints); m_allocator->Free(m_triangleVelocityConstraints); - m_allocator->Free(m_particlePositionConstraints); - m_allocator->Free(m_particleVelocityConstraints); - m_allocator->Free(m_bodyPositionConstraints); m_allocator->Free(m_bodyVelocityConstraints); } void b3ClothContactSolver::InitializeBodyContactConstraints() { - b3DenseVec3& x = *m_positions; - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - vc->indexA = c->p1->m_solverId; - vc->bodyB = c->s2->GetBody(); + vc->indexA = c->m_p1->m_solverId; + vc->bodyB = c->m_s2->GetBody(); - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + vc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; vc->invMassB = vc->bodyB->GetInverseMass(); vc->invIA.SetZero(); vc->invIB = vc->bodyB->GetWorldInverseInertia(); - vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction()); + vc->friction = b3MixFriction(c->m_p1->m_friction, c->m_s2->GetFriction()); - pc->indexA = c->p1->m_solverId; + pc->indexA = c->m_p1->m_solverId; pc->bodyB = vc->bodyB; - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + pc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; pc->invMassB = vc->bodyB->m_invMass; pc->invIA.SetZero(); pc->invIB = vc->bodyB->m_worldInvI; - pc->radiusA = c->p1->m_radius; - pc->radiusB = c->s2->m_radius; + pc->radiusA = c->m_p1->m_radius; + pc->radiusB = c->m_s2->m_radius; pc->localCenterA.SetZero(); pc->localCenterB = pc->bodyB->m_sweep.localCenter; - pc->localPointA = c->localPoint1; - pc->localPointB = c->localPoint2; + pc->normalA = c->m_normal1; + pc->localPointA = c->m_localPoint1; + pc->localPointB = c->m_localPoint2; } for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; @@ -114,7 +105,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() b3Mat33 iA = vc->invIA; b3Mat33 iB = vc->invIB; - b3Vec3 xA = x[indexA]; + b3Vec3 xA = m_positions[indexA]; b3Vec3 xB = bodyB->m_sweep.worldCenter; b3Quat qA; qA.SetIdentity(); @@ -131,12 +122,12 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() xfB.rotation = b3QuatMat33(qB); xfB.position = xB - b3Mul(xfB.rotation, localCenterB); - b3BodyContactWorldPoint wp; + b3ParticleBodyContactWorldPoint wp; wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB); vc->normal = wp.normal; - vc->tangent1 = c->t1; - vc->tangent2 = c->t2; + vc->tangent1 = c->m_tangent1; + vc->tangent2 = c->m_tangent2; vc->point = wp.point; b3Vec3 point = vc->point; @@ -147,8 +138,8 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() vc->rA = rA; vc->rB = rB; - vc->normalImpulse = c->normalImpulse; - vc->tangentImpulse = c->tangentImpulse; + vc->normalImpulse = c->m_normalImpulse; + vc->tangentImpulse = c->m_tangentImpulse; { b3Vec3 n = vc->normal; @@ -185,128 +176,44 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() } } } - -void b3ClothContactSolver::InitializeParticleContactConstraints() -{ - b3DenseVec3& x = *m_positions; - b3DenseVec3& v = *m_velocities; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - vc->indexA = c->p1->m_solverId; - vc->indexB = c->p2->m_solverId; - - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction); - - pc->indexA = c->p1->m_solverId; - pc->indexB = c->p2->m_solverId; - - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - pc->radiusA = c->p1->m_radius; - pc->radiusB = c->p2->m_radius; - } - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - - b3ParticleContactWorldPoint wp; - wp.Initialize(c); - - vc->normal = wp.normal; - vc->tangent1 = c->t1; - vc->tangent2 = c->t2; - vc->point = wp.point; - - b3Vec3 point = vc->point; - - vc->normalImpulse = c->normalImpulse; - vc->tangentImpulse = c->tangentImpulse; - - { - b3Vec3 n = vc->normal; - - float32 K = mA + mB; - - vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - - vc->velocityBias = 0.0f; - } - - { - b3Vec3 t1 = vc->tangent1; - b3Vec3 t2 = vc->tangent2; - - float32 k11 = mA + mB; - float32 k12 = 0.0f; - float32 k22 = mA + mB; - - b3Mat22 K; - K.x.Set(k11, k12); - K.y.Set(k12, k22); - - vc->tangentMass = b3Inverse(K); - } - } -} - void b3ClothContactSolver::InitializeTriangleContactConstraints() { - b3DenseVec3& x = *m_positions; - for (u32 i = 0; i < m_triangleContactCount; ++i) { - b3TriangleContact* c = m_triangleContacts[i]; + b3ParticleTriangleContact* c = m_triangleContacts[i]; b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i; - vc->indexA = c->p1->m_solverId; - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + vc->indexA = c->m_p1->m_solverId; + vc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; - vc->indexB = c->p2->m_solverId; - vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + vc->indexB = c->m_p2->m_solverId; + vc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass; - vc->indexC = c->p3->m_solverId; - vc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; + vc->indexC = c->m_p3->m_solverId; + vc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass; - vc->indexD = c->p4->m_solverId; - vc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; + vc->indexD = c->m_p4->m_solverId; + vc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass; - pc->indexA = c->p1->m_solverId; - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - pc->radiusA = c->p1->m_radius; + pc->indexA = c->m_p1->m_solverId; + pc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass; + pc->radiusA = c->m_p1->m_radius; - pc->indexB = c->p2->m_solverId; - pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + pc->indexB = c->m_p2->m_solverId; + pc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass; - pc->indexC = c->p3->m_solverId; - pc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; + pc->indexC = c->m_p3->m_solverId; + pc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass; - pc->indexD = c->p4->m_solverId; - pc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; + pc->indexD = c->m_p4->m_solverId; + pc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass; - pc->triangleRadius = 0.0f; - pc->front = c->front; + pc->triangleRadius = c->m_t2->m_radius; + + pc->wB = c->m_w2; + pc->wC = c->m_w3; + pc->wD = c->m_w4; u32 indexA = pc->indexA; float32 mA = pc->invMassA; @@ -320,47 +227,42 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints() u32 indexD = pc->indexD; float32 mD = pc->invMassD; - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; + b3Vec3 xA = m_positions[indexA]; + b3Vec3 xB = m_positions[indexB]; + b3Vec3 xC = m_positions[indexC]; + b3Vec3 xD = m_positions[indexD]; - b3Vec3 n = b3Cross(xC - xB, xD - xB); + float32 wB = pc->wB; + float32 wC = pc->wC; + float32 wD = pc->wD; - if (pc->front == false) - { - n = -n; - } + b3Vec3 cB = wB * xB + wC * xC + wD * xD; - float32 n_len = n.Normalize(); + b3Vec3 n = cB - xA; + n.Normalize(); - b3Mat33 I; I.SetIdentity(); + vc->normal = n; + vc->wB = wB; + vc->wC = wC; + vc->wD = wD; - b3Mat33 N = I - b3Outer(n, n); - if (n_len > B3_EPSILON) - { - N = (1.0f / n_len) * N; - } - - b3Vec3 N_n = N * n; - - vc->JA = n; - vc->JC = b3Cross(xD - xB, N_n); - vc->JD = b3Cross(xC - xB, N_n); - vc->JB = b3Cross(xC - xD, N_n) - n; - - // Compute effective mass. - float32 K = mA + mB + mC + mD; + float32 K = mA + mB * wB * wB + mC * wC * wC + mD * wD * wD; vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - vc->normalImpulse = c->normalImpulse; + vc->normalImpulse = c->m_normalImpulse; + + vc->friction = b3MixFriction(c->m_p1->m_friction, c->m_t2->m_friction); + vc->tangent1 = b3Perp(n); + vc->tangent2 = b3Cross(vc->tangent1, n); + vc->tangentMass1 = vc->normalMass; + vc->tangentMass2 = vc->normalMass; + vc->tangentImpulse1 = c->m_tangentImpulse1; + vc->tangentImpulse2 = c->m_tangentImpulse2; } } -void b3ClothContactSolver::WarmStart() +void b3ClothContactSolver::WarmStartBodyContactConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; @@ -368,7 +270,7 @@ void b3ClothContactSolver::WarmStart() u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; - b3Vec3 vA = v[indexA]; + b3Vec3 vA = m_velocities[indexA]; b3Vec3 vB = bodyB->GetLinearVelocity(); b3Vec3 wA; wA.SetZero(); @@ -397,40 +299,15 @@ void b3ClothContactSolver::WarmStart() vB += mB * (P1 + P2); wB += iB * b3Cross(vc->rB, P1 + P2); - v[indexA] = vA; + m_velocities[indexA] = vA; bodyB->SetLinearVelocity(vB); bodyB->SetAngularVelocity(wB); } +} - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 P = vc->normalImpulse * vc->normal; - - vA -= mA * P; - vB += mB * P; - - b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1; - b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2; - - vA -= mA * (P1 + P2); - vB += mB * (P1 + P2); - - v[indexA] = vA; - v[indexB] = vB; - } - +void b3ClothContactSolver::WarmStartTriangleContactConstraints() +{ for (u32 i = 0; i < m_triangleContactCount; ++i) { b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; @@ -440,37 +317,76 @@ void b3ClothContactSolver::WarmStart() u32 indexC = vc->indexC; u32 indexD = vc->indexD; - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; + b3Vec3 vA = m_velocities[indexA]; + b3Vec3 vB = m_velocities[indexB]; + b3Vec3 vC = m_velocities[indexC]; + b3Vec3 vD = m_velocities[indexD]; float32 mA = vc->invMassA; float32 mB = vc->invMassB; float32 mC = vc->invMassC; float32 mD = vc->invMassD; - b3Vec3 PA = vc->normalImpulse * vc->JA; - b3Vec3 PB = vc->normalImpulse * vc->JB; - b3Vec3 PC = vc->normalImpulse * vc->JC; - b3Vec3 PD = vc->normalImpulse * vc->JD; + { + b3Vec3 JA = -vc->normal; + b3Vec3 JB = vc->wB * vc->normal; + b3Vec3 JC = vc->wC * vc->normal; + b3Vec3 JD = vc->wD * vc->normal; - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; + b3Vec3 PA = vc->normalImpulse * JA; + b3Vec3 PB = vc->normalImpulse * JB; + b3Vec3 PC = vc->normalImpulse * JC; + b3Vec3 PD = vc->normalImpulse * JD; - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + { + b3Vec3 JA = -vc->tangent1; + b3Vec3 JB = vc->wB * vc->tangent1; + b3Vec3 JC = vc->wC * vc->tangent1; + b3Vec3 JD = vc->wD * vc->tangent1; + + b3Vec3 PA = vc->tangentImpulse1 * JA; + b3Vec3 PB = vc->tangentImpulse1 * JB; + b3Vec3 PC = vc->tangentImpulse1 * JC; + b3Vec3 PD = vc->tangentImpulse1 * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + { + b3Vec3 JA = -vc->tangent2; + b3Vec3 JB = vc->wB * vc->tangent2; + b3Vec3 JC = vc->wC * vc->tangent2; + b3Vec3 JD = vc->wD * vc->tangent2; + + b3Vec3 PA = vc->tangentImpulse2 * JA; + b3Vec3 PB = vc->tangentImpulse2 * JB; + b3Vec3 PC = vc->tangentImpulse2 * JC; + b3Vec3 PD = vc->tangentImpulse2 * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + m_velocities[indexA] = vA; + m_velocities[indexB] = vB; + m_velocities[indexC] = vC; + m_velocities[indexD] = vD; } } void b3ClothContactSolver::SolveBodyContactVelocityConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; @@ -478,7 +394,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; - b3Vec3 vA = v[indexA]; + b3Vec3 vA = m_velocities[indexA]; b3Vec3 vB = bodyB->GetLinearVelocity(); b3Vec3 wA; wA.SetZero(); @@ -548,88 +464,15 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() wB += iB * b3Cross(rB, P); } - v[indexA] = vA; + m_velocities[indexA] = vA; bodyB->SetLinearVelocity(vB); bodyB->SetAngularVelocity(wB); } } -void b3ClothContactSolver::SolveParticleContactVelocityConstraints() -{ - b3DenseVec3& v = *m_velocities; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 normal = vc->normal; - b3Vec3 point = vc->point; - - // Solve normal constraint. - { - b3Vec3 dv = vB - vA; - 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; - vB += mB * P; - } - - // Solve tangent constraints. - { - b3Vec3 dv = vB - vA; - - 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; - vB += mB * P; - } - - v[indexA] = vA; - v[indexB] = vB; - } -} - void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_triangleContactCount; ++i) { b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; @@ -646,36 +489,109 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() u32 indexD = vc->indexD; float32 mD = vc->invMassD; - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; + float32 wB = vc->wB; + float32 wC = vc->wC; + float32 wD = vc->wD; - b3Vec3 n = vc->JA; + b3Vec3 vA = m_velocities[indexA]; + b3Vec3 vB = m_velocities[indexB]; + b3Vec3 vC = m_velocities[indexC]; + b3Vec3 vD = m_velocities[indexD]; - // Allow some slop and prevent large corrections. - float32 Cdot = b3Dot(n, vA - vB); + // Solve normal constraint. + { + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; - float32 impulse = -vc->normalMass * Cdot; + float32 Cdot = b3Dot(vCB - vA, vc->normal); - float32 oldImpulse = vc->normalImpulse; - vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); - impulse = vc->normalImpulse - oldImpulse; + float32 impulse = -vc->normalMass * Cdot; - b3Vec3 PA = impulse * vc->JA; - b3Vec3 PB = impulse * vc->JB; - b3Vec3 PC = impulse * vc->JC; - b3Vec3 PD = impulse * vc->JD; + float32 oldImpulse = vc->normalImpulse; + vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); + impulse = vc->normalImpulse - oldImpulse; - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; + b3Vec3 JA = -vc->normal; + b3Vec3 JB = wB * vc->normal; + b3Vec3 JC = wC * vc->normal; + b3Vec3 JD = wD * vc->normal; - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + // Solve tangent constraint. + { + float32 hi = vc->friction * vc->normalImpulse; + float32 lo = -hi; + + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; + + float32 Cdot = b3Dot(vCB - vA, vc->tangent1); + + float32 impulse = -vc->tangentMass1 * Cdot; + + float32 oldImpulse = vc->tangentImpulse1; + vc->tangentImpulse1 = b3Clamp(vc->tangentImpulse1 + impulse, lo, hi); + impulse = vc->tangentImpulse1 - oldImpulse; + + b3Vec3 JA = -vc->tangent1; + b3Vec3 JB = wB * vc->tangent1; + b3Vec3 JC = wC * vc->tangent1; + b3Vec3 JD = wD * vc->tangent1; + + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + // Solve bitangent constraint. + { + float32 hi = vc->friction * vc->normalImpulse; + float32 lo = -hi; + + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; + + float32 Cdot = b3Dot(vCB - vA, vc->tangent2); + + float32 impulse = -vc->tangentMass2 * Cdot; + + float32 oldImpulse = vc->tangentImpulse2; + vc->tangentImpulse2 = b3Clamp(vc->tangentImpulse2 + impulse, lo, hi); + impulse = vc->tangentImpulse2 - oldImpulse; + + b3Vec3 JA = -vc->tangent2; + b3Vec3 JB = wB * vc->tangent2; + b3Vec3 JC = wC * vc->tangent2; + b3Vec3 JD = wD * vc->tangent2; + + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + m_velocities[indexA] = vA; + m_velocities[indexB] = vB; + m_velocities[indexC] = vC; + m_velocities[indexD] = vD; } } @@ -683,28 +599,21 @@ void b3ClothContactSolver::StoreImpulses() { for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; - } - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; + c->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse = vc->tangentImpulse; } for (u32 i = 0; i < m_triangleContactCount; ++i) { - b3TriangleContact* c = m_triangleContacts[i]; + b3ParticleTriangleContact* c = m_triangleContacts[i]; b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; - c->normalImpulse = vc->normalImpulse; + c->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse1 = vc->tangentImpulse1; + c->m_tangentImpulse2 = vc->tangentImpulse2; } } @@ -712,27 +621,20 @@ struct b3ClothSolverBodyContactSolverPoint { void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) { + b3Vec3 nA = pc->normalA; + b3Vec3 cA = b3Mul(xfA, pc->localPointA); b3Vec3 cB = b3Mul(xfB, pc->localPointB); float32 rA = pc->radiusA; float32 rB = pc->radiusB; - - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - + b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA; - point = 0.5f * (pA + pB); + point = cB; normal = nA; - separation = distance - rA - rB; + separation = b3Dot(cB - cA, nA) - rA - rB; } b3Vec3 normal; @@ -742,8 +644,6 @@ struct b3ClothSolverBodyContactSolverPoint bool b3ClothContactSolver::SolveBodyContactPositionConstraints() { - b3DenseVec3& x = *m_positions; - float32 minSeparation = 0.0f; for (u32 i = 0; i < m_bodyContactCount; ++i) @@ -760,7 +660,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() b3Mat33 iB = pc->invIB; b3Vec3 localCenterB = pc->localCenterB; - b3Vec3 cA = x[indexA]; + b3Vec3 cA = m_positions[indexA]; b3Quat qA; qA.SetIdentity(); b3Vec3 cB = bodyB->m_sweep.worldCenter; @@ -808,7 +708,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() qB += b3Derivative(qB, iB * b3Cross(rB, P)); qB.Normalize(); - x[indexA] = cA; + m_positions[indexA] = cA; bodyB->m_sweep.worldCenter = cB; bodyB->m_sweep.orientation = qB; @@ -817,87 +717,8 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() return minSeparation >= -3.0f * B3_LINEAR_SLOP; } -struct b3ClothSolverParticleContactSolverPoint -{ - void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB) - { - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = distance - rA - rB; - } - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; - -bool b3ClothContactSolver::SolveParticleContactPositionConstraints() -{ - b3DenseVec3& x = *m_positions; - - float32 minSeparation = 0.0f; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - u32 indexA = pc->indexA; - float32 mA = pc->invMassA; - float32 rA = pc->radiusA; - - u32 indexB = pc->indexB; - float32 mB = pc->invMassB; - float32 rB = pc->radiusB; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - - b3ClothSolverParticleContactSolverPoint cpcp; - cpcp.Initialize(xA, rA, xB, rB); - - 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. - float32 K = mA + mB; - - // Compute normal impulse. - float32 impulse = K > 0.0f ? -C / K : 0.0f; - b3Vec3 P = impulse * normal; - - xA -= mA * P; - xB += mB * P; - - x[indexA] = xA; - x[indexB] = xB; - } - - return minSeparation >= -3.0f * B3_LINEAR_SLOP; -} - bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() { - b3DenseVec3& x = *m_positions; - float32 minSeparation = 0.0f; for (u32 i = 0; i < m_triangleContactCount; ++i) @@ -919,20 +740,21 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() float32 triangleRadius = pc->triangleRadius; - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; + float32 wB = pc->wB; + float32 wC = pc->wC; + float32 wD = pc->wD; - b3Vec3 n = b3Cross(xC - xB, xD - xB); - if (pc->front == false) - { - n = -n; - } + b3Vec3 xA = m_positions[indexA]; + b3Vec3 xB = m_positions[indexB]; + b3Vec3 xC = m_positions[indexC]; + b3Vec3 xD = m_positions[indexD]; - float32 n_len = n.Normalize(); + b3Vec3 cB = wB * xB + wC * xC + wD * xD; + + b3Vec3 n = cB - xA; + + float32 distance = n.Normalize(); - float32 distance = b3Dot(n, xA - xB); float32 separation = distance - radiusA - triangleRadius; // Update max constraint error. @@ -941,23 +763,13 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() // Allow some slop and prevent large corrections. float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); - b3Mat33 I; I.SetIdentity(); - - b3Mat33 N = I - b3Outer(n, n); - if (n_len > B3_EPSILON) - { - N = (1.0f / n_len) * N; - } - - b3Vec3 N_n = N * n; - - b3Vec3 JA = n; - b3Vec3 JC = b3Cross(xD - xB, N_n); - b3Vec3 JD = b3Cross(xC - xB, N_n); - b3Vec3 JB = b3Cross(xC - xD, N_n) - n; + b3Vec3 JA = -n; + b3Vec3 JB = wB * n; + b3Vec3 JC = wC * n; + b3Vec3 JD = wD * n; // Compute effective mass. - float32 K = mA + mB + mC + mD; + float32 K = mA + mB * wB * wB + mC * wC * wC + mD * wD * wD; // Compute normal impulse. float32 impulse = K > 0.0f ? -C / K : 0.0f; @@ -972,10 +784,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() xC += mC * PC; xD += mD * PD; - x[indexA] = xA; - x[indexB] = xB; - x[indexC] = xC; - x[indexD] = xD; + m_positions[indexA] = xA; + m_positions[indexB] = xB; + m_positions[indexC] = xC; + m_positions[indexD] = xD; } return minSeparation >= -3.0f * B3_LINEAR_SLOP; diff --git a/src/bounce/cloth/contacts/cloth_particle_body_contact.cpp b/src/bounce/cloth/contacts/cloth_particle_body_contact.cpp new file mode 100644 index 0000000..fb580f9 --- /dev/null +++ b/src/bounce/cloth/contacts/cloth_particle_body_contact.cpp @@ -0,0 +1,62 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +void b3ParticleBodyContact::Update() +{ + b3Sphere sphere; + sphere.radius = m_p1->m_radius; + sphere.vertex = m_p1->m_position; + + b3Shape* shape = m_s2; + b3Body* body = shape->GetBody(); + b3Transform xf = body->GetTransform(); + + b3TestSphereOutput out; + if (shape->TestSphere(&out, sphere, xf) == false) + { + m_active = false; + return; + } + + m_active = true; + m_normal1 = -out.normal; + m_localPoint1.SetZero(); + m_localPoint2 = body->GetLocalPoint(out.point); + m_tangent1 = b3Perp(m_normal1); + m_tangent2 = b3Cross(m_tangent1, m_normal1); +} + +void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) +{ + b3Vec3 nA = c->m_normal1; + + b3Vec3 cA = xfA * c->m_localPoint1; + b3Vec3 cB = xfB * c->m_localPoint2; + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = b3Dot(cB - cA, nA) - rA - rB; +} diff --git a/src/bounce/cloth/contacts/cloth_particle_triangle_contact.cpp b/src/bounce/cloth/contacts/cloth_particle_triangle_contact.cpp new file mode 100644 index 0000000..64d4073 --- /dev/null +++ b/src/bounce/cloth/contacts/cloth_particle_triangle_contact.cpp @@ -0,0 +1,179 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include + +// Solve constrained Barycentric coordinates for point Q +static void b3Solve3(float32 out[3], + const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, + const b3Vec3& Q) +{ + // Test vertex regions + float32 wAB[3], wBC[3], wCA[3]; + b3BarycentricCoordinates(wAB, A, B, Q); + b3BarycentricCoordinates(wBC, B, C, Q); + b3BarycentricCoordinates(wCA, C, A, Q); + + // R A + if (wAB[1] <= 0.0f && wCA[0] <= 0.0f) + { + out[0] = 1.0f; + out[1] = 0.0f; + out[2] = 0.0f; + return; + } + + // R B + if (wAB[0] <= 0.0f && wBC[1] <= 0.0f) + { + out[0] = 0.0f; + out[1] = 1.0f; + out[2] = 0.0f; + return; + } + + // R C + if (wBC[0] <= 0.0f && wCA[1] <= 0.0f) + { + out[0] = 0.0f; + out[1] = 0.0f; + out[2] = 1.0f; + return; + } + + // Test edge regions + float32 wABC[4]; + b3BarycentricCoordinates(wABC, A, B, C, Q); + + // R AB + if (wAB[0] > 0.0f && wAB[1] > 0.0f && wABC[3] * wABC[2] <= 0.0f) + { + float32 divisor = wAB[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = s * wAB[0]; + out[1] = s * wAB[1]; + out[2] = 0.0f; + return; + } + + // R BC + if (wBC[0] > 0.0f && wBC[1] > 0.0f && wABC[3] * wABC[0] <= 0.0f) + { + float32 divisor = wBC[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = 0.0f; + out[1] = s * wBC[0]; + out[2] = s * wBC[1]; + return; + } + + // R CA + if (wCA[0] > 0.0f && wCA[1] > 0.0f && wABC[3] * wABC[1] <= 0.0f) + { + float32 divisor = wCA[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = s * wCA[1]; + out[1] = 0.0f; + out[2] = s * wCA[0]; + return; + } + + // R ABC/ACB + float32 divisor = wABC[3]; + if (divisor == 0.0f) + { + float32 s = 1.0f / 3.0f; + out[0] = s; + out[1] = s; + out[2] = s; + return; + } + + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = s * wABC[0]; + out[1] = s * wABC[1]; + out[2] = s * wABC[2]; +} + +void b3ParticleTriangleContact::Update() +{ + b3Vec3 A = m_p2->m_position; + b3Vec3 B = m_p3->m_position; + b3Vec3 C = m_p4->m_position; + + b3Vec3 N = b3Cross(B - A, C - A); + float32 len = N.Normalize(); + + // Is ABC degenerate? + if (len == 0.0f) + { + m_active = false; + return; + } + + float32 r1 = m_p1->m_radius; + float32 r2 = m_t2->m_radius; + + float32 totalRadius = r1 + r2; + + b3Vec3 P1 = m_p1->m_position; + + float32 distance = b3Dot(N, P1 - A); + + // Is P1 below the plane? + if (distance < -totalRadius) + { + m_active = false; + return; + } + + // Is P1 above the plane? + if (distance > totalRadius) + { + m_active = false; + return; + } + + // Closest point on ABC to P1 + float32 wABC[3]; + b3Solve3(wABC, A, B, C, P1); + + b3Vec3 P2 = wABC[0] * A + wABC[1] * B + wABC[2] * C; + + if (b3DistanceSquared(P1, P2) > totalRadius * totalRadius) + { + m_active = false; + return; + } + + // Activate the contact + m_active = true; + + // Store Barycentric coordinates for P1 + m_w2 = wABC[0]; + m_w3 = wABC[1]; + m_w4 = wABC[2]; +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/force.cpp b/src/bounce/cloth/forces/force.cpp similarity index 59% rename from src/bounce/dynamics/cloth/force.cpp rename to src/bounce/cloth/forces/force.cpp index 0365c66..c1368b5 100644 --- a/src/bounce/dynamics/cloth/force.cpp +++ b/src/bounce/cloth/forces/force.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,25 +16,39 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include +#include +#include +#include +#include +#include b3Force* b3Force::Create(const b3ForceDef* def) { b3Force* force = NULL; switch (def->type) { + case e_strechForce: + { + void* block = b3Alloc(sizeof(b3StrechForce)); + force = new (block) b3StrechForce((b3StrechForceDef*)def); + break; + } + case e_shearForce: + { + void* block = b3Alloc(sizeof(b3ShearForce)); + force = new (block) b3ShearForce((b3ShearForceDef*)def); + break; + } case e_springForce: { void* block = b3Alloc(sizeof(b3SpringForce)); force = new (block) b3SpringForce((b3SpringForceDef*)def); break; } - case e_bendForce: + case e_mouseForce: { - void* block = b3Alloc(sizeof(b3BendForce)); - force = new (block) b3BendForce((b3BendForceDef*)def); + void* block = b3Alloc(sizeof(b3MouseForce)); + force = new (block) b3MouseForce((b3MouseForceDef*)def); break; } default: @@ -53,6 +67,20 @@ void b3Force::Destroy(b3Force* force) b3ForceType type = force->GetType(); switch (type) { + case e_strechForce: + { + b3StrechForce* o = (b3StrechForce*)force; + o->~b3StrechForce(); + b3Free(force); + break; + } + case e_shearForce: + { + b3ShearForce* o = (b3ShearForce*)force; + o->~b3ShearForce(); + b3Free(force); + break; + } case e_springForce: { b3SpringForce* o = (b3SpringForce*)force; @@ -60,10 +88,10 @@ void b3Force::Destroy(b3Force* force) b3Free(force); break; } - case e_bendForce: + case e_mouseForce: { - b3BendForce* o = (b3BendForce*)force; - o->~b3BendForce(); + b3MouseForce* o = (b3MouseForce*)force; + o->~b3MouseForce(); b3Free(force); break; } diff --git a/src/bounce/cloth/forces/mouse_force.cpp b/src/bounce/cloth/forces/mouse_force.cpp new file mode 100644 index 0000000..51810b7 --- /dev/null +++ b/src/bounce/cloth/forces/mouse_force.cpp @@ -0,0 +1,228 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3MouseForce::b3MouseForce(const b3MouseForceDef* def) +{ + m_type = e_mouseForce; + m_particle = def->particle; + m_triangle = def->triangle; + m_w2 = def->w2; + m_w3 = def->w3; + m_w4 = def->w4; + m_km = def->mouse; + m_kd = def->damping; + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + m_f4.SetZero(); +} + +b3MouseForce::~b3MouseForce() +{ + +} + +bool b3MouseForce::HasParticle(const b3Particle* particle) const +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + return m_particle == particle || p1 == particle || p2 == particle || p3 == particle; +} + +void b3MouseForce::Apply(const b3ClothForceSolverData* data) +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + b3Particle* p1 = m_particle; + b3Particle* p2 = cloth->m_particles[triangle->v1]; + b3Particle* p3 = cloth->m_particles[triangle->v2]; + b3Particle* p4 = cloth->m_particles[triangle->v3]; + + u32 i1 = p1->m_solverId; + u32 i2 = p2->m_solverId; + u32 i3 = p3->m_solverId; + u32 i4 = p4->m_solverId; + + b3DenseVec3& x = *data->x; + b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; + + b3Vec3 x1 = x[i1]; + b3Vec3 x2 = x[i2]; + b3Vec3 x3 = x[i3]; + b3Vec3 x4 = x[i4]; + + b3Vec3 v1 = v[i1]; + b3Vec3 v2 = v[i2]; + b3Vec3 v3 = v[i3]; + b3Vec3 v4 = v[i4]; + + b3Mat33 I; I.SetIdentity(); + + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + m_f4.SetZero(); + + b3Vec3 c2 = m_w2 * x2 + m_w3 * x3 + m_w4 * x4; + + b3Vec3 d = x1 - c2; + float32 len = b3Length(d); + + if (len > 0.0f) + { + b3Vec3 n = d / len; + + // Jacobian + b3Vec3 dCdx[4]; + dCdx[0] = n; + dCdx[1] = -m_w2 * n; + dCdx[2] = -m_w3 * n; + dCdx[3] = -m_w4 * n; + + if (m_km > 0.0f) + { + float32 C = len; + + // Force + b3Vec3 fs[4]; + for (u32 i = 0; i < 4; ++i) + { + fs[i] = -m_km * C * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + m_f4 += fs[3]; + + // Force derivative + b3Mat33 K[4][4]; + for (u32 i = 0; i < 4; ++i) + { + for (u32 j = 0; j < 4; ++j) + { + //b3Mat33 d2Cvxij; + //b3Mat33 Kij = -m_km * (b3Outer(dCdx[i], dCdx[j]) + C * d2Cvxij); + b3Mat33 Kij = -m_km * b3Outer(dCdx[i], dCdx[j]); + + K[i][j] = Kij; + } + } + + dfdx(i1, i1) += K[0][0]; + dfdx(i1, i2) += K[0][1]; + dfdx(i1, i3) += K[0][2]; + dfdx(i1, i4) += K[0][3]; + + dfdx(i2, i1) += K[1][0]; + dfdx(i2, i2) += K[1][1]; + dfdx(i2, i3) += K[1][2]; + dfdx(i2, i4) += K[1][3]; + + dfdx(i3, i1) += K[2][0]; + dfdx(i3, i2) += K[2][1]; + dfdx(i3, i3) += K[2][2]; + dfdx(i3, i4) += K[2][3]; + + dfdx(i4, i1) += K[3][0]; + dfdx(i4, i2) += K[3][1]; + dfdx(i4, i3) += K[3][2]; + dfdx(i4, i4) += K[3][3]; + } + + if (m_kd > 0.0f) + { + b3Vec3 vs[4] = { v1, v2, v3, v4 }; + + float32 dCdt = 0.0f; + for (u32 i = 0; i < 4; ++i) + { + dCdt += b3Dot(dCdx[i], vs[i]); + } + + // Force + b3Vec3 fs[4]; + for (u32 i = 0; i < 4; ++i) + { + fs[i] = -m_kd * dCdt * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + m_f4 += fs[3]; + + // Force derivative + b3Mat33 K[4][4]; + for (u32 i = 0; i < 4; ++i) + { + for (u32 j = 0; j < 4; ++j) + { + b3Mat33 Kij = -m_kd * b3Outer(dCdx[i], dCdx[j]); + + K[i][j] = Kij; + } + } + + dfdv(i1, i1) += K[0][0]; + dfdv(i1, i2) += K[0][1]; + dfdv(i1, i3) += K[0][2]; + dfdv(i1, i4) += K[0][3]; + + dfdv(i2, i1) += K[1][0]; + dfdv(i2, i2) += K[1][1]; + dfdv(i2, i3) += K[1][2]; + dfdv(i2, i4) += K[1][3]; + + dfdv(i3, i1) += K[2][0]; + dfdv(i3, i2) += K[2][1]; + dfdv(i3, i3) += K[2][2]; + dfdv(i3, i4) += K[2][3]; + + dfdv(i4, i1) += K[3][0]; + dfdv(i4, i2) += K[3][1]; + dfdv(i4, i3) += K[3][2]; + dfdv(i4, i4) += K[3][3]; + } + } + + f[i1] += m_f1; + f[i2] += m_f2; + f[i3] += m_f3; + f[i4] += m_f4; +} \ No newline at end of file diff --git a/src/bounce/cloth/forces/shear_force.cpp b/src/bounce/cloth/forces/shear_force.cpp new file mode 100644 index 0000000..07a35f4 --- /dev/null +++ b/src/bounce/cloth/forces/shear_force.cpp @@ -0,0 +1,212 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3ShearForce::b3ShearForce(const b3ShearForceDef* def) +{ + m_type = e_shearForce; + m_triangle = def->triangle; + m_ks = def->shearing; + m_kd = def->damping; + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); +} + +b3ShearForce::~b3ShearForce() +{ + +} + +bool b3ShearForce::HasParticle(const b3Particle* particle) const +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + return p1 == particle || p2 == particle || p3 == particle; +} + +void b3ShearForce::Apply(const b3ClothForceSolverData* data) +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + float32 alpha = m_triangle->m_alpha; + float32 du1 = m_triangle->m_du1; + float32 dv1 = m_triangle->m_dv1; + float32 du2 = m_triangle->m_du2; + float32 dv2 = m_triangle->m_dv2; + float32 inv_det = m_triangle->m_inv_det; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + u32 i1 = p1->m_solverId; + u32 i2 = p2->m_solverId; + u32 i3 = p3->m_solverId; + + b3DenseVec3& x = *data->x; + b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; + + b3Vec3 x1 = x[i1]; + b3Vec3 x2 = x[i2]; + b3Vec3 x3 = x[i3]; + + b3Vec3 v1 = v[i1]; + b3Vec3 v2 = v[i2]; + b3Vec3 v3 = v[i3]; + + b3Mat33 I; I.SetIdentity(); + + b3Vec3 dx1 = x2 - x1; + b3Vec3 dx2 = x3 - x1; + + b3Vec3 wu = inv_det * (dv2 * dx1 - dv1 * dx2); + b3Vec3 wv = inv_det * (-du2 * dx1 + du1 * dx2); + + b3Vec3 dwudx; + dwudx[0] = inv_det * (dv1 - dv2); + dwudx[1] = inv_det * dv2; + dwudx[2] = -inv_det * dv1; + + b3Vec3 dwvdx; + dwvdx[0] = inv_det * (du2 - du1); + dwvdx[1] = -inv_det * du2; + dwvdx[2] = inv_det * du1; + + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + + // Jacobian + b3Vec3 dCdx[3]; + for (u32 i = 0; i < 3; ++i) + { + dCdx[i] = alpha * (dwudx[i] * wv + dwvdx[i] * wu); + } + + if (m_ks > 0.0f) + { + float32 C = alpha * b3Dot(wu, wv); + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_ks * C * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + //b3Mat33 d2Cxij = alpha * (dwudx[i] * dwvdx[j] + dwudx[j] * dwvdx[i]) * I; + //b3Mat33 Kij = -m_ks * (b3Outer(dCdx[i], dCdx[j]) + C * d2Cxij); + b3Mat33 Kij = -m_ks * b3Outer(dCdx[i], dCdx[j]); + + K[i][j] = Kij; + } + } + + dfdx(i1, i1) += K[0][0]; + dfdx(i1, i2) += K[0][1]; + dfdx(i1, i3) += K[0][2]; + + dfdx(i2, i1) += K[1][0]; + dfdx(i2, i2) += K[1][1]; + dfdx(i2, i3) += K[1][2]; + + dfdx(i3, i1) += K[2][0]; + dfdx(i3, i2) += K[2][1]; + dfdx(i3, i3) += K[2][2]; + } + + if (m_kd > 0.0f) + { + b3Vec3 vs[3] = { v1, v2, v3 }; + + float32 dCdt = 0.0f; + for (u32 i = 0; i < 3; ++i) + { + dCdt += b3Dot(dCdx[i], vs[i]); + } + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_kd * dCdt * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 Kij = -m_kd * b3Outer(dCdx[i], dCdx[j]); + + K[i][j] = Kij; + } + } + + dfdv(i1, i1) += K[0][0]; + dfdv(i1, i2) += K[0][1]; + dfdv(i1, i3) += K[0][2]; + + dfdv(i2, i1) += K[1][0]; + dfdv(i2, i2) += K[1][1]; + dfdv(i2, i3) += K[1][2]; + + dfdv(i3, i1) += K[2][0]; + dfdv(i3, i2) += K[2][1]; + dfdv(i3, i3) += K[2][2]; + } + + f[i1] += m_f1; + f[i2] += m_f2; + f[i3] += m_f3; +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/spring_force.cpp b/src/bounce/cloth/forces/spring_force.cpp similarity index 65% rename from src/bounce/dynamics/cloth/spring_force.cpp rename to src/bounce/cloth/forces/spring_force.cpp index 25ba831..a037254 100644 --- a/src/bounce/dynamics/cloth/spring_force.cpp +++ b/src/bounce/cloth/forces/spring_force.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,11 +16,11 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include void b3SpringForceDef::Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness) { @@ -50,13 +50,13 @@ b3SpringForce::~b3SpringForce() } -void b3SpringForce::Apply(const b3ClothSolverData* data) +void b3SpringForce::Apply(const b3ClothForceSolverData* data) { b3DenseVec3& x = *data->x; b3DenseVec3& v = *data->v; b3DenseVec3& f = *data->f; - b3SparseSymMat33& dfdx = *data->dfdx; - b3SparseSymMat33& dfdv = *data->dfdv; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; u32 i1 = m_p1->m_solverId; u32 i2 = m_p2->m_solverId; @@ -77,42 +77,42 @@ void b3SpringForce::Apply(const b3ClothSolverData* data) float32 L = b3Length(dx); - if (L >= m_L0) + if (L > m_L0) { - // Apply tension - b3Vec3 n = dx / L; - - m_f += -m_ks * (L - m_L0) * n; - // Jacobian - b3Mat33 Jx11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); - b3Mat33 Jx12 = -Jx11; - //b3Mat33 Jx21 = Jx12; - b3Mat33 Jx22 = Jx11; + b3Vec3 dCdx = dx / L; - dfdx(i1, i1) += Jx11; - dfdx(i1, i2) += Jx12; - //dfdx(i2, i1) += Jx21; - dfdx(i2, i2) += Jx22; + m_f += -m_ks * (L - m_L0) * dCdx; + + // Force derivative + b3Mat33 K11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); + b3Mat33 K12 = -K11; + b3Mat33 K21 = K12; + b3Mat33 K22 = K11; + + dfdx(i1, i1) += K11; + dfdx(i1, i2) += K12; + dfdx(i2, i1) += K21; + dfdx(i2, i2) += K22; } } if (m_kd > 0.0f) { - // Apply damping + // C * J b3Vec3 dv = v1 - v2; m_f += -m_kd * dv; - b3Mat33 Jv11 = -m_kd * I; - b3Mat33 Jv12 = -Jv11; - //b3Mat33 Jv21 = Jv12; - b3Mat33 Jv22 = Jv11; + b3Mat33 K11 = -m_kd * I; + b3Mat33 K12 = -K11; + b3Mat33 K21 = K12; + b3Mat33 K22 = K11; - dfdv(i1, i1) += Jv11; - dfdv(i1, i2) += Jv12; - //dfdv(i2, i1) += Jv21; - dfdv(i2, i2) += Jv22; + dfdv(i1, i1) += K11; + dfdv(i1, i2) += K12; + dfdv(i2, i1) += K21; + dfdv(i2, i2) += K22; } f[i1] += m_f; diff --git a/src/bounce/cloth/forces/strech_force.cpp b/src/bounce/cloth/forces/strech_force.cpp new file mode 100644 index 0000000..65dec70 --- /dev/null +++ b/src/bounce/cloth/forces/strech_force.cpp @@ -0,0 +1,329 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3StrechForce::b3StrechForce(const b3StrechForceDef* def) +{ + m_type = e_strechForce; + m_triangle = def->triangle; + m_ks = def->streching; + m_kd = def->damping; + m_bu = def->bu; + m_bv = def->bv; + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); +} + +b3StrechForce::~b3StrechForce() +{ + +} + +bool b3StrechForce::HasParticle(const b3Particle* particle) const +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + return p1 == particle || p2 == particle || p3 == particle; +} + +void b3StrechForce::Apply(const b3ClothForceSolverData* data) +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + float32 alpha = m_triangle->m_alpha; + float32 du1 = m_triangle->m_du1; + float32 dv1 = m_triangle->m_dv1; + float32 du2 = m_triangle->m_du2; + float32 dv2 = m_triangle->m_dv2; + float32 inv_det = m_triangle->m_inv_det; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + u32 i1 = p1->m_solverId; + u32 i2 = p2->m_solverId; + u32 i3 = p3->m_solverId; + + b3DenseVec3& x = *data->x; + b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; + + b3Vec3 x1 = x[i1]; + b3Vec3 x2 = x[i2]; + b3Vec3 x3 = x[i3]; + + b3Vec3 v1 = v[i1]; + b3Vec3 v2 = v[i2]; + b3Vec3 v3 = v[i3]; + + b3Mat33 I; I.SetIdentity(); + + b3Vec3 dx1 = x2 - x1; + b3Vec3 dx2 = x3 - x1; + + b3Vec3 wu = inv_det * (dv2 * dx1 - dv1 * dx2); + float32 len_wu = b3Length(wu); + + b3Vec3 wv = inv_det * (-du2 * dx1 + du1 * dx2); + float32 len_wv = b3Length(wv); + + b3Vec3 dwudx; + dwudx[0] = inv_det * (dv1 - dv2); + dwudx[1] = inv_det * dv2; + dwudx[2] = -inv_det * dv1; + + b3Vec3 dwvdx; + dwvdx[0] = inv_det * (du2 - du1); + dwvdx[1] = -inv_det * du2; + dwvdx[2] = inv_det * du1; + + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + + if (len_wu > 0.0f) + { + float32 inv_len_wu = 1.0f / len_wu; + b3Vec3 n_wu = inv_len_wu * wu; + + // Jacobian + b3Vec3 dCudx[3]; + for (u32 i = 0; i < 3; ++i) + { + dCudx[i] = alpha * dwudx[i] * n_wu; + } + + if (m_ks > 0.0f) + { + if (len_wu > m_bu) + { + float32 Cu = alpha * (len_wu - m_bu); + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_ks * Cu * dCudx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 d2Cuxij = (alpha * inv_len_wu * dwudx[i] * dwudx[j]) * (I - b3Outer(n_wu, n_wu)); + + b3Mat33 Kij = -m_ks * (b3Outer(dCudx[i], dCudx[j]) + Cu * d2Cuxij); + + K[i][j] = Kij; + } + } + + dfdx(i1, i1) += K[0][0]; + dfdx(i1, i2) += K[0][1]; + dfdx(i1, i3) += K[0][2]; + + dfdx(i2, i1) += K[1][0]; + dfdx(i2, i2) += K[1][1]; + dfdx(i2, i3) += K[1][2]; + + dfdx(i3, i1) += K[2][0]; + dfdx(i3, i2) += K[2][1]; + dfdx(i3, i3) += K[2][2]; + } + } + + if (m_kd > 0.0f) + { + b3Vec3 vs[3] = { v1, v2, v3 }; + float32 dCudt = 0.0f; + for (u32 i = 0; i < 3; ++i) + { + dCudt += b3Dot(dCudx[i], vs[i]); + } + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_kd * dCudt * dCudx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 Kij = -m_kd * b3Outer(dCudx[i], dCudx[j]); + + K[i][j] = Kij; + } + } + + dfdv(i1, i1) += K[0][0]; + dfdv(i1, i2) += K[0][1]; + dfdv(i1, i3) += K[0][2]; + + dfdv(i2, i1) += K[1][0]; + dfdv(i2, i2) += K[1][1]; + dfdv(i2, i3) += K[1][2]; + + dfdv(i3, i1) += K[2][0]; + dfdv(i3, i2) += K[2][1]; + dfdv(i3, i3) += K[2][2]; + } + } + + if (len_wv > 0.0f) + { + float32 inv_len_wv = 1.0f / len_wv; + b3Vec3 n_wv = inv_len_wv * wv; + + // Jacobian + b3Vec3 dCvdx[3]; + for (u32 i = 0; i < 3; ++i) + { + dCvdx[i] = alpha * dwvdx[i] * n_wv; + } + + if (m_ks > 0.0f) + { + if (len_wv > m_bv) + { + float32 Cv = alpha * (len_wv - m_bv); + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_ks * Cv * dCvdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 d2Cvxij = (alpha * inv_len_wv * dwvdx[i] * dwvdx[j]) * (I - b3Outer(n_wv, n_wv)); + + b3Mat33 Kij = -m_ks * (b3Outer(dCvdx[i], dCvdx[j]) + Cv * d2Cvxij); + + K[i][j] = Kij; + } + } + + dfdx(i1, i1) += K[0][0]; + dfdx(i1, i2) += K[0][1]; + dfdx(i1, i3) += K[0][2]; + + dfdx(i2, i1) += K[1][0]; + dfdx(i2, i2) += K[1][1]; + dfdx(i2, i3) += K[1][2]; + + dfdx(i3, i1) += K[2][0]; + dfdx(i3, i2) += K[2][1]; + dfdx(i3, i3) += K[2][2]; + } + } + + if (m_kd > 0.0f) + { + b3Vec3 vs[3] = { v1, v2, v3 }; + + float32 dCvdt = 0.0f; + for (u32 i = 0; i < 3; ++i) + { + dCvdt += b3Dot(dCvdx[i], vs[i]); + } + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_kd * dCvdt * dCvdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Force derivative + b3Mat33 K[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 Kij = -m_kd * b3Outer(dCvdx[i], dCvdx[j]); + + K[i][j] = Kij; + } + } + + dfdv(i1, i1) += K[0][0]; + dfdv(i1, i2) += K[0][1]; + dfdv(i1, i3) += K[0][2]; + + dfdv(i2, i1) += K[1][0]; + dfdv(i2, i2) += K[1][1]; + dfdv(i2, i3) += K[1][2]; + + dfdv(i3, i1) += K[2][0]; + dfdv(i3, i2) += K[2][1]; + dfdv(i3, i3) += K[2][2]; + } + } + + f[i1] += m_f1; + f[i2] += m_f2; + f[i3] += m_f3; +} \ No newline at end of file diff --git a/src/bounce/garment/garment_mesh.cpp b/src/bounce/cloth/garment/garment_mesh.cpp similarity index 77% rename from src/bounce/garment/garment_mesh.cpp rename to src/bounce/cloth/garment/garment_mesh.cpp index 05faf52..0fb4433 100644 --- a/src/bounce/garment/garment_mesh.cpp +++ b/src/bounce/cloth/garment/garment_mesh.cpp @@ -16,9 +16,9 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include +#include +#include +#include #define ANSI_DECLARATORS #define REAL double @@ -65,7 +65,12 @@ static void b3Set(b3SewingPatternMesh* mesh, float32 desiredArea, const b3Sewing in.pointlist[i] = (REAL)fp[i]; } in.pointattributelist = NULL; - in.pointmarkerlist = NULL; + in.pointmarkerlist = (int*)malloc(pattern->vertexCount * sizeof(int)); + for (u32 i = 0; i < pattern->vertexCount; ++i) + { + in.pointmarkerlist[i] = 10 + int(i); + } + in.numberofpoints = pattern->vertexCount; in.numberofpointattributes = 0; @@ -101,10 +106,11 @@ static void b3Set(b3SewingPatternMesh* mesh, float32 desiredArea, const b3Sewing mid.segmentmarkerlist = NULL; // Run triangulation + // Q - quiet // z - zero based indices // p - PSLG // c - preserve the convex hull - triangulate("zpc", &in, &mid, NULL); + triangulate("Qzpc", &in, &mid, NULL); // Refine @@ -126,11 +132,24 @@ static void b3Set(b3SewingPatternMesh* mesh, float32 desiredArea, const b3Sewing out.segmentmarkerlist = NULL; // Run triangulation + // Q - quiet // z - zero based indices // p - PSLG // c - preserve the convex hull // r - read triangles - triangulate("zpcra", &mid, &out, NULL); + triangulate("Qzpcra", &mid, &out, NULL); + + // The first vertices of the output structure must be the vertices of the input structure. + for (int i = 0; i < in.numberofpoints; ++i) + { + B3_ASSERT(in.pointmarkerlist[i] == out.pointmarkerlist[i]); + } + + // Note: comparing doubles + for (int i = 0; i < in.numberofpoints * 2; ++i) + { + B3_ASSERT(in.pointlist[i] == out.pointlist[i]); + } // Convert the output structure @@ -159,6 +178,7 @@ static void b3Set(b3SewingPatternMesh* mesh, float32 desiredArea, const b3Sewing // Free the input structure free(in.pointlist); + free(in.pointmarkerlist); // Free the middle structure free(mid.pointlist); @@ -186,4 +206,21 @@ void b3GarmentMesh::Set(b3Garment* g, float32 desiredArea) { b3Set(meshes + i, desiredArea, garment->patterns[i]); } + + // It's okay to do run the following code because + // the first vertices of a sewing pattern mesh are the vertices of its + // corresponding sewing pattern. + sewingCount = garment->sewingCount; + sewingLines = (b3GarmentMeshSewingLine*)b3Alloc(garment->sewingCount * sizeof(b3GarmentMeshSewingLine)); + for (u32 i = 0; i < garment->sewingCount; ++i) + { + b3SewingLine* sewingLine = garment->sewingLines + i; + b3GarmentMeshSewingLine* line = sewingLines + i; + + line->s1 = sewingLine->p1; + line->v1 = sewingLine->v1; + + line->s2 = sewingLine->p2; + line->v2 = sewingLine->v2; + } } diff --git a/src/bounce/dynamics/cloth/cloth_mesh.cpp b/src/bounce/cloth/garment_cloth_mesh.cpp similarity index 94% rename from src/bounce/dynamics/cloth/cloth_mesh.cpp rename to src/bounce/cloth/garment_cloth_mesh.cpp index f51c408..5f09f49 100644 --- a/src/bounce/dynamics/cloth/cloth_mesh.cpp +++ b/src/bounce/cloth/garment_cloth_mesh.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,10 +16,10 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include -#include +#include +#include +#include +#include b3GarmentClothMesh::b3GarmentClothMesh() { diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp new file mode 100644 index 0000000..c1da933 --- /dev/null +++ b/src/bounce/cloth/particle.cpp @@ -0,0 +1,144 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) +{ + m_cloth = cloth; + m_type = def.type; + m_position = def.position; + m_velocity = def.velocity; + m_force = def.force; + m_translation.SetZero(); + m_mass = def.mass; + + if (m_mass == 0.0f) + { + m_type = e_staticParticle; + m_mass = 1.0f; + m_invMass = 1.0f; + } + else + { + m_type = e_dynamicParticle; + m_invMass = 1.0f / m_mass; + } + + m_radius = def.radius; + m_friction = def.friction; + m_userData = nullptr; + m_x.SetZero(); + m_vertex = ~0; +} + +b3Particle::~b3Particle() +{ + +} + +void b3Particle::Synchronize(const b3Vec3& displacement) +{ + b3AABB3 aabb; + aabb.Set(m_position, m_radius); + + m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_broadPhaseId, aabb, displacement); +} + +void b3Particle::SynchronizeTriangles() +{ + if (m_vertex == ~0) + { + return; + } + + for (u32 i = 0; i < m_cloth->m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + i; + + if (triangle->v1 == m_vertex || triangle->v2 == m_vertex || triangle->v3 == m_vertex) + { + m_cloth->GetTriangle(i)->Synchronize(b3Vec3_zero); + } + } +} + +void b3Particle::DestroyContacts() +{ + { + // Destroy body contacts + b3ParticleBodyContact* c = m_cloth->m_contactManager.m_particleBodyContactList.m_head; + while (c) + { + if (c->m_p1 == this) + { + b3ParticleBodyContact* quack = c; + c = c->m_next; + m_cloth->m_contactManager.Destroy(quack); + continue; + } + + c = c->m_next; + } + } + + { + // Destroy triangle contacts + b3ParticleTriangleContact* c = m_cloth->m_contactManager.m_particleTriangleContactList.m_head; + while (c) + { + if (c->m_p1 == this) + { + b3ParticleTriangleContact* quack = c; + c = c->m_next; + m_cloth->m_contactManager.Destroy(quack); + continue; + } + + c = c->m_next; + } + } +} + +void b3Particle::SetType(b3ParticleType type) +{ + if (m_type == type) + { + return; + } + + m_type = type; + m_force.SetZero(); + + if (type == e_staticParticle) + { + m_velocity.SetZero(); + m_translation.SetZero(); + + Synchronize(b3Vec3_zero); + SynchronizeTriangles(); + } + + DestroyContacts(); + + // Move the proxy so new contacts can be created. + m_cloth->m_contactManager.m_broadPhase.TouchProxy(m_broadPhaseId); +} \ No newline at end of file diff --git a/src/bounce/collision/broad_phase.cpp b/src/bounce/collision/broad_phase.cpp index 83372bd..6f0d5bb 100644 --- a/src/bounce/collision/broad_phase.cpp +++ b/src/bounce/collision/broad_phase.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -20,6 +20,8 @@ b3BroadPhase::b3BroadPhase() { + m_proxyCount = 0; + m_moveBufferCapacity = 16; m_moveBuffer = (u32*)b3Alloc(m_moveBufferCapacity * sizeof(u32)); memset(m_moveBuffer, 0, m_moveBufferCapacity * sizeof(u32)); @@ -57,6 +59,17 @@ void b3BroadPhase::BufferMove(u32 proxyId) ++m_moveBufferCount; } +void b3BroadPhase::UnbufferMove(u32 proxyId) +{ + for (u32 i = 0; i < m_moveBufferCount; ++i) + { + if (m_moveBuffer[i] == proxyId) + { + m_moveBuffer[i] = B3_NULL_PROXY; + } + } +} + bool b3BroadPhase::TestOverlap(u32 proxy1, u32 proxy2) const { return m_tree.TestOverlap(proxy1, proxy2); @@ -64,20 +77,23 @@ bool b3BroadPhase::TestOverlap(u32 proxy1, u32 proxy2) const u32 b3BroadPhase::CreateProxy(const b3AABB3& aabb, void* userData) { - // Later, if the node aabb has changed then it should be reinserted into the tree. - // However, this can be expansive due to the hierarchy reconstruction. - // Therefore, the original AABB is extended and inserted into the tree, - // so we can check later if the new (original) AABB is inside the old (fat) AABB. b3AABB3 fatAABB = aabb; fatAABB.Extend(B3_AABB_EXTENSION); + u32 proxyId = m_tree.InsertNode(fatAABB, userData); + + ++m_proxyCount; + BufferMove(proxyId); + return proxyId; } void b3BroadPhase::DestroyProxy(u32 proxyId) { - return m_tree.RemoveNode(proxyId); + UnbufferMove(proxyId); + --m_proxyCount; + m_tree.RemoveNode(proxyId); } bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement) @@ -88,39 +104,38 @@ bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& dis return false; } - // Update the tree with a fat and motion predicted AABB. - const b3Vec3 kExtension(B3_AABB_EXTENSION, B3_AABB_EXTENSION, B3_AABB_EXTENSION); + // Extend the AABB. + b3AABB3 fatAABB = aabb; + fatAABB.Extend(B3_AABB_EXTENSION); - // Extend the new (original) AABB. - b3AABB3 fatAABB; - fatAABB.m_lower = aabb.m_lower - kExtension; - fatAABB.m_upper = aabb.m_upper + kExtension; + // Predict AABB displacement. + b3Vec3 d = B3_AABB_MULTIPLIER * displacement; - if (displacement.x < 0.0f) + if (d.x < 0.0f) { - fatAABB.m_lower.x += B3_AABB_MULTIPLIER * displacement.x; + fatAABB.m_lower.x += d.x; } else { - fatAABB.m_upper.x += B3_AABB_MULTIPLIER * displacement.x; + fatAABB.m_upper.x += d.x; } - if (displacement.y < 0.0f) + if (d.y < 0.0f) { - fatAABB.m_lower.y += B3_AABB_MULTIPLIER * displacement.y; + fatAABB.m_lower.y += d.y; } else { - fatAABB.m_upper.y += B3_AABB_MULTIPLIER * displacement.y; + fatAABB.m_upper.y += d.y; } - if (displacement.z < 0.0f) + if (d.z < 0.0f) { - fatAABB.m_lower.z += B3_AABB_MULTIPLIER * displacement.z; + fatAABB.m_lower.z += d.z; } else { - fatAABB.m_upper.z += B3_AABB_MULTIPLIER * displacement.z; + fatAABB.m_upper.z += d.z; } // Update proxy with the extented AABB. @@ -133,6 +148,11 @@ bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& dis return true; } +void b3BroadPhase::TouchProxy(u32 proxyId) +{ + BufferMove(proxyId); +} + bool b3BroadPhase::Report(u32 proxyId) { if (proxyId == m_queryProxyId) diff --git a/src/bounce/collision/collision.cpp b/src/bounce/collision/collision.cpp index 9826e31..9246b4f 100644 --- a/src/bounce/collision/collision.cpp +++ b/src/bounce/collision/collision.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,6 +16,8 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#include + #include #include #include @@ -24,4 +26,90 @@ const b3Sphere b3Sphere_identity(b3Vec3_zero, 1.0f); const b3Capsule b3Capsule_identity(b3Vec3(0.0f, -0.5f, 0.0f), b3Vec3(0.0f, 0.5f, 0.0f), 1.0f); -const b3BoxHull b3BoxHull_identity(1.0f, 1.0f, 1.0f); \ No newline at end of file +const b3BoxHull b3BoxHull_identity(1.0f, 1.0f, 1.0f); + +bool b3RayCast(b3RayCastOutput* output, + const b3RayCastInput* input, + const b3Vec3& v1, const b3Vec3& v2, const b3Vec3& v3) +{ + b3Vec3 p1 = input->p1; + b3Vec3 p2 = input->p2; + float32 maxFraction = input->maxFraction; + + b3Vec3 d = p2 - p1; + + if (b3LengthSquared(d) < B3_EPSILON * B3_EPSILON) + { + return false; + } + + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); + float32 len = b3Length(n); + + if (len == 0.0f) + { + return false; + } + + n /= len; + + float32 num = b3Dot(n, v1 - p1); + float32 den = b3Dot(n, d); + + if (den == 0.0f) + { + return false; + } + + float32 fraction = num / den; + + // Is the intersection not on the segment? + if (fraction < 0.0f || maxFraction < fraction) + { + return false; + } + + b3Vec3 Q = p1 + fraction * d; + + b3Vec3 A = v1; + b3Vec3 B = v2; + b3Vec3 C = v3; + + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + b3Vec3 QC = C - Q; + + b3Vec3 QB_x_QC = b3Cross(QB, QC); + b3Vec3 QC_x_QA = b3Cross(QC, QA); + b3Vec3 QA_x_QB = b3Cross(QA, QB); + + b3Vec3 AB_x_AC = b3Cross(AB, AC); + + // Barycentric coordinates for Q + float32 u = b3Dot(QB_x_QC, AB_x_AC); + float32 v = b3Dot(QC_x_QA, AB_x_AC); + float32 w = b3Dot(QA_x_QB, AB_x_AC); + + // Is the intersection on the triangle? + if (u >= 0.0f && v >= 0.0f && w >= 0.0f) + { + output->fraction = fraction; + + // Does the ray start from below or above the triangle? + if (num > 0.0f) + { + output->normal = -n; + } + else + { + output->normal = n; + } + + return true; + } + + return false; +} diff --git a/src/bounce/collision/gjk/gjk.cpp b/src/bounce/collision/gjk/gjk.cpp index cfa8b59..ed3a024 100644 --- a/src/bounce/collision/gjk/gjk.cpp +++ b/src/bounce/collision/gjk/gjk.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -29,7 +29,7 @@ u32 b3_gjkCalls = 0, b3_gjkIters = 0, b3_gjkMaxIters = 0; // Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v) // with respect to a segment AB. // The last output value is the divisor. -static B3_FORCE_INLINE void b3Barycentric(float32 out[3], +static B3_FORCE_INLINE void b3Barycentric(float32 out[3], const b3Vec3& A, const b3Vec3& B, const b3Vec3& Q) { @@ -38,7 +38,7 @@ static B3_FORCE_INLINE void b3Barycentric(float32 out[3], b3Vec3 QB = B - Q; //float32 divisor = b3Dot(AB, AB); - + out[0] = b3Dot(QB, AB); out[1] = -b3Dot(QA, AB); out[2] = out[0] + out[1]; @@ -63,7 +63,7 @@ static B3_FORCE_INLINE void b3Barycentric(float32 out[4], b3Vec3 QA_x_QB = b3Cross(QA, QB); b3Vec3 AB_x_AC = b3Cross(AB, AC); - + //float32 divisor = b3Dot(AB_x_AC, AB_x_AC); out[0] = b3Dot(QB_x_QC, AB_x_AC); @@ -248,7 +248,7 @@ void b3Simplex::Solve3(const b3Vec3& Q) b3Barycentric(wAB, A.point, B.point, Q); b3Barycentric(wBC, B.point, C.point, Q); b3Barycentric(wCA, C.point, A.point, Q); - + // R A if (wAB[1] <= 0.0f && wCA[0] <= 0.0f) { @@ -279,7 +279,7 @@ void b3Simplex::Solve3(const b3Vec3& Q) // Test edge regions float32 wABC[4]; b3Barycentric(wABC, A.point, B.point, C.point, Q); - + // This is used to help testing if the face degenerates // into an edge. float32 area = wABC[3]; @@ -361,7 +361,7 @@ void b3Simplex::Solve4(const b3Vec3& Q) b3Barycentric(wAD, A.point, D.point, Q); b3Barycentric(wCD, C.point, D.point, Q); b3Barycentric(wDB, D.point, B.point, Q); - + // R A if (wAB[1] <= 0.0f && wAC[1] <= 0.0f && wAD[1] <= 0.0f) { @@ -848,4 +848,268 @@ b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1, b3SimplexCache cache; cache.count = 0; return b3GJK(xf1, proxy1, xf2, proxy2, false, &cache); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +// Brian Mirtich +// "Conservative Advancement" +bool b3GJKShapeCast(b3GJKShapeCastOutput* output, + const b3Transform& xf1, const b3GJKProxy& proxy1, + const b3Transform& xf2, const b3GJKProxy& proxy2, const b3Vec3& translation2) +{ + float32 r1 = proxy1.radius; + float32 r2 = proxy2.radius; + float32 radius = r1 + r2; + + float32 bound = b3Length(translation2); + B3_ASSERT(bound > 0.0f); + + float32 t = 0.0f; + b3SimplexCache cache; + cache.count = 0; + b3GJKOutput gjkOut = b3GJK(xf1, proxy1, xf2, proxy2, false, &cache); + float32 d = gjkOut.distance; + + if (d == 0.0f) + { + // Overlap + output->iterations = 0; + return false; + } + + const float32 tolerance = 0.25f * B3_LINEAR_SLOP; + + b3Vec3 n = gjkOut.point2 - gjkOut.point1; + n /= d; + + if (d < radius + tolerance) + { + // Touch + output->t = t; + output->point = gjkOut.point1 + r1 * n; + output->normal = n; + output->iterations = 0; + return true; + } + + u32 iter = 0; + for (;;) + { + ++iter; + + B3_ASSERT(d >= radius); + float32 dt = (d - radius) / bound; + t += dt; + + if (t >= 1.0f) + { + // No overlap + output->iterations = iter; + return false; + } + + b3Transform txf1 = xf1; + + b3Transform txf2; + txf2.rotation = xf2.rotation; + txf2.position = (1.0f - t) * xf2.position + t * (xf2.position + translation2); + + gjkOut = b3GJK(txf1, proxy1, txf2, proxy2, false, &cache); + d = gjkOut.distance; + + if (d == 0.0f) + { + break; + } + + n = gjkOut.point2 - gjkOut.point1; + n /= d; + + if (d < radius + tolerance) + { + break; + } + } + + if (d > 0.0f) + { + n = gjkOut.point2 - gjkOut.point1; + n /= d; + } + + output->t = t; + output->point = gjkOut.point1 + r1 * n; + output->normal = n; + output->iterations = iter; + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +// Gino van der Bergen +// "Smooth Mesh Contacts with GJK" +// Game Physics Pearls 2010, page 99 +bool b3GJKShapeCastCSO(b3GJKShapeCastOutput* output, + const b3Transform& xf1, const b3GJKProxy& proxy1, + const b3Transform& xf2, const b3GJKProxy& proxy2, const b3Vec3& translation2) +{ + float32 r1 = proxy1.radius; + float32 r2 = proxy2.radius; + float32 radius = r1 + r2; + + b3Vec3 r = translation2; + + float32 t = 0.0f; + b3Vec3 n = b3Vec3_zero; + + u32 index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -r)); + u32 index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, r)); + b3Vec3 w1 = xf1 * proxy1.GetVertex(index1); + b3Vec3 w2 = xf2 * proxy2.GetVertex(index2); + b3Vec3 v = w1 - w2; + + b3Simplex simplex; + simplex.m_count = 0; + + b3SimplexVertex* vertices = simplex.m_vertices; + + u32 save1[4], save2[4]; + u32 saveCount = 0; + + const u32 kMaxIters = 20; + const float32 kTolerance = 10.0f * B3_EPSILON; + + float32 maxTolerance = 1.0f; + + u32 iter = 0; + while (iter < kMaxIters && b3Abs(b3LengthSquared(v) - radius * radius) > kTolerance * maxTolerance) + { + // Support in direction -v + index1 = proxy1.GetSupportIndex(b3MulT(xf1.rotation, -v)); + index2 = proxy2.GetSupportIndex(b3MulT(xf2.rotation, v)); + w1 = xf1 * proxy1.GetVertex(index1); + w2 = xf2 * proxy2.GetVertex(index2); + b3Vec3 p = w1 - w2; + + // Support plane on boundary of CSO is (-v, p) + // -v is normal at p + float32 vp = b3Dot(v, p); + float32 vr = b3Dot(v, r); + + if (vp - radius > t * vr) + { + if (vr > 0.0f) + { + t = (vp - radius) / vr; + + if (t > 1.0f) + { + output->iterations = iter; + return false; + } + + n = -v; + + // Flush the simplex + simplex.m_count = 0; + saveCount = 0; + } + else + { + output->iterations = iter; + return false; + } + } + + // Unite p - s to simplex + b3Vec3 s = t * r; + + b3SimplexVertex* vertex = vertices + simplex.m_count; + vertex->index1 = index1; + vertex->point1 = w1; + vertex->index2 = index2; + vertex->point2 = w2; + vertex->point = p - s; + + // If we found a duplicate support point we must exit to avoid cycling. + bool duplicate = false; + for (u32 i = 0; i < saveCount; ++i) + { + if (vertex->index1 == save1[i] && vertex->index2 == save2[i]) + { + duplicate = true; + break; + } + } + + if (duplicate) + { + break; + } + + ++simplex.m_count; + + // Compute tolerance + maxTolerance = -B3_EPSILON; + for (u32 i = 0; i < simplex.m_count; ++i) + { + maxTolerance = b3Max(maxTolerance, b3LengthSquared(vertices[i].point)); + } + + // Copy simplex so we can identify duplicates. + saveCount = simplex.m_count; + for (u32 i = 0; i < saveCount; ++i) + { + save1[i] = vertices[i].index1; + save2[i] = vertices[i].index2; + } + + // Sub-solve + const b3Vec3 origin = b3Vec3_zero; + + switch (simplex.m_count) + { + case 1: + break; + case 2: + simplex.Solve2(origin); + break; + case 3: + simplex.Solve3(origin); + break; + case 4: + simplex.Solve4(origin); + break; + default: + B3_ASSERT(false); + break; + } + + if (simplex.m_count == 4) + { + break; + } + + v = simplex.GetClosestPoint(); + + ++iter; + } + + // Prepare output. + b3Vec3 point1, point2; + simplex.GetClosestPoints(&point1, &point2); + + if (b3LengthSquared(v) > B3_EPSILON * B3_EPSILON) + { + n = -v; + } + + n.Normalize(); + + output->t = t; + output->point = point1 + r1 * n; + output->normal = n; + output->iterations = iter; + return true; } \ No newline at end of file diff --git a/src/bounce/collision/gjk/gjk_feature_pair.cpp b/src/bounce/collision/gjk/gjk_feature_pair.cpp index 34ada7d..42a0283 100644 --- a/src/bounce/collision/gjk/gjk_feature_pair.cpp +++ b/src/bounce/collision/gjk/gjk_feature_pair.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/collision/sat/sat.cpp b/src/bounce/collision/sat/sat.cpp index 565cabf..6d38811 100644 --- a/src/bounce/collision/sat/sat.cpp +++ b/src/bounce/collision/sat/sat.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/collision/sat/sat_edge_and_hull.cpp b/src/bounce/collision/sat/sat_edge_and_hull.cpp index fe639df..3df7dc5 100644 --- a/src/bounce/collision/sat/sat_edge_and_hull.cpp +++ b/src/bounce/collision/sat/sat_edge_and_hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/collision/sat/sat_vertex_and_hull.cpp b/src/bounce/collision/sat/sat_vertex_and_hull.cpp index 96a4b81..10a90ce 100644 --- a/src/bounce/collision/sat/sat_vertex_and_hull.cpp +++ b/src/bounce/collision/sat/sat_vertex_and_hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/collision/shapes/hull.cpp b/src/bounce/collision/shapes/hull.cpp index 873604a..74cecd7 100644 --- a/src/bounce/collision/shapes/hull.cpp +++ b/src/bounce/collision/shapes/hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/collision/shapes/qhull.cpp b/src/bounce/collision/shapes/qhull.cpp index 1db841b..1c5b259 100644 --- a/src/bounce/collision/shapes/qhull.cpp +++ b/src/bounce/collision/shapes/qhull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -19,6 +19,9 @@ #include #include +#include +#include + template struct b3UniqueStackArray { @@ -335,8 +338,12 @@ void b3QHull::Set(u32 vtxStride, const void* vtxBase, u32 vtxCount, bool simplif ++iface; } + vertices = hullVertices.Begin(); vertexCount = hullVertices.Count(); + edges = hullEdges.Begin(); edgeCount = hullEdges.Count(); + faces = hullFaces.Begin(); + planes = hullPlanes.Begin(); faceCount = hullFaces.Count(); // Validate @@ -348,35 +355,17 @@ void b3QHull::Set(u32 vtxStride, const void* vtxBase, u32 vtxCount, bool simplif void b3QHull::SetAsSphere(float32 radius) { - enum + B3_ASSERT(radius > 0.0f); + + smMesh mesh; + smCreateMesh(mesh, 2); + + for (u32 i = 0; i < mesh.vertexCount; ++i) { - e_rings = 8, - e_sectors = 8, - e_vertexCount = e_rings * e_sectors - }; - - float32 R = 1.0f / float32(e_rings - 1); - float32 S = 1.0f / float32(e_sectors - 1); - - b3Vec3 vs[e_vertexCount]; - - u32 vc = 0; - for (u32 r = 0; r < e_rings; r++) - { - for (u32 s = 0; s < e_sectors; s++) - { - float32 y = sin(-0.5f * B3_PI + B3_PI * r * R); - float32 x = cos(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - float32 z = sin(2.0f * B3_PI * s * S) * sin(B3_PI * r * R); - - vs[vc].Set(x, y, z); - vs[vc] *= radius; - ++vc; - } + mesh.vertices[i] *= radius; } - // Set - Set(sizeof(b3Vec3), vs, e_vertexCount, false); + Set(sizeof(b3Vec3), mesh.vertices, mesh.vertexCount, false); } void b3QHull::SetAsCylinder(float32 radius, float32 ey) @@ -384,51 +373,19 @@ void b3QHull::SetAsCylinder(float32 radius, float32 ey) B3_ASSERT(radius > 0.0f); B3_ASSERT(ey > 0.0f); - const u32 kEdgeCount = 20; - const u32 kVertexCount = 4 * kEdgeCount; - b3Vec3 vs[kVertexCount]; + float32 height = 2.0f * ey; - u32 count = 0; - - float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); - b3Quat q = b3QuatRotationY(kAngleInc); + cymMesh mesh; + cymCreateMesh(mesh, 20); + for (u32 i = 0; i < mesh.vertexCount; ++i) { - b3Vec3 center(0.0f, -ey, 0.0f); - b3Vec3 n1(1.0f, 0.0f, 0.0f); - b3Vec3 v1 = center + radius * n1; - for (u32 i = 0; i < kEdgeCount; ++i) - { - b3Vec3 n2 = b3Mul(q, n1); - b3Vec3 v2 = center + radius * n2; - - vs[count++] = v1; - vs[count++] = v2; - - n1 = n2; - v1 = v2; - } + mesh.vertices[i].x *= radius; + mesh.vertices[i].y *= height; + mesh.vertices[i].z *= radius; } - { - b3Vec3 center(0.0f, ey, 0.0f); - b3Vec3 n1(1.0f, 0.0f, 0.0f); - b3Vec3 v1 = center + radius * n1; - for (u32 i = 0; i < kEdgeCount; ++i) - { - b3Vec3 n2 = b3Mul(q, n1); - b3Vec3 v2 = center + radius * n2; - - vs[count++] = v1; - vs[count++] = v2; - - n1 = n2; - v1 = v2; - } - } - - // Set - Set(sizeof(b3Vec3), vs, count, false); + Set(sizeof(b3Vec3), mesh.vertices, mesh.vertexCount, false); } void b3QHull::SetAsCone(float32 radius, float32 ey) diff --git a/src/bounce/collision/trees/dynamic_tree.cpp b/src/bounce/collision/trees/dynamic_tree.cpp index c609677..ca30f1c 100644 --- a/src/bounce/collision/trees/dynamic_tree.cpp +++ b/src/bounce/collision/trees/dynamic_tree.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -123,6 +123,7 @@ void b3DynamicTree::RemoveNode(u32 proxyId) { // Remove from the tree. RemoveLeaf(proxyId); + // Remove from the node array and make it available. FreeNode(proxyId); } @@ -131,8 +132,10 @@ void b3DynamicTree::UpdateNode(u32 proxyId, const b3AABB3& aabb) { B3_ASSERT(m_root != B3_NULL_NODE_D); B3_ASSERT(m_nodes[proxyId].IsLeaf()); + // Remove old AABB from the tree. RemoveLeaf(proxyId); + // Insert the new AABB to the tree. m_nodes[proxyId].aabb = aabb; InsertLeaf(proxyId); diff --git a/src/bounce/collision/trees/static_tree.cpp b/src/bounce/collision/trees/static_tree.cpp index 490cfa0..7dd0436 100644 --- a/src/bounce/collision/trees/static_tree.cpp +++ b/src/bounce/collision/trees/static_tree.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/common/math/math.cpp b/src/bounce/common/math/math.cpp index f50ff18..793e5c5 100644 --- a/src/bounce/common/math/math.cpp +++ b/src/bounce/common/math/math.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/common/memory/block_pool.cpp b/src/bounce/common/memory/block_pool.cpp index 307c39f..e6a0619 100644 --- a/src/bounce/common/memory/block_pool.cpp +++ b/src/bounce/common/memory/block_pool.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/common/memory/stack_allocator.cpp b/src/bounce/common/memory/stack_allocator.cpp index f158ff9..1077899 100644 --- a/src/bounce/common/memory/stack_allocator.cpp +++ b/src/bounce/common/memory/stack_allocator.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/common/settings.cpp b/src/bounce/common/settings.cpp index 8da10c4..9c663c4 100644 --- a/src/bounce/common/settings.cpp +++ b/src/bounce/common/settings.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/body.cpp b/src/bounce/dynamics/body.cpp index 9116d14..caf19c8 100644 --- a/src/bounce/dynamics/body.cpp +++ b/src/bounce/dynamics/body.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -457,7 +457,7 @@ void b3Body::SetType(b3BodyType type) b3BroadPhase* phase = &m_world->m_contactMan.m_broadPhase; for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next) { - phase->BufferMove(s->m_broadPhaseID); + phase->TouchProxy(s->m_broadPhaseID); } } diff --git a/src/bounce/dynamics/cloth/bend_force.cpp b/src/bounce/dynamics/cloth/bend_force.cpp deleted file mode 100644 index d540706..0000000 --- a/src/bounce/dynamics/cloth/bend_force.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -void b3BendForceDef::Initialize(b3Particle* particle1, b3Particle* particle2, b3Particle* particle3, b3Particle* particle4, float32 structuralStiffness, float32 dampingStiffness) -{ - type = e_bendForce; - p1 = particle1; - p2 = particle2; - p3 = particle3; - p4 = particle4; - - b3Vec3 x1 = p1->GetPosition(); - b3Vec3 x2 = p2->GetPosition(); - b3Vec3 x3 = p3->GetPosition(); - b3Vec3 x4 = p4->GetPosition(); - - restDistance = b3Distance(x2, x3); - structural = structuralStiffness; - damping = dampingStiffness; -} - -b3BendForce::b3BendForce(const b3BendForceDef* def) -{ - m_type = e_bendForce; - m_p1 = def->p1; - m_p2 = def->p2; - m_p3 = def->p3; - m_p4 = def->p4; - m_L0 = def->restDistance; - m_angle0 = def->restAngle; - m_ks = def->structural; - m_kd = def->damping; -} - -b3BendForce::~b3BendForce() -{ - -} - -void b3BendForce::Apply(const b3ClothSolverData* data) -{ - b3DenseVec3& x = *data->x; - b3DenseVec3& v = *data->v; - b3DenseVec3& f = *data->f; - b3SparseSymMat33& dfdx = *data->dfdx; - b3SparseSymMat33& dfdv = *data->dfdv; - - u32 i1 = m_p2->m_solverId; - u32 i2 = m_p3->m_solverId; - - b3Vec3 x1 = x[i1]; - b3Vec3 v1 = v[i1]; - - b3Vec3 x2 = x[i2]; - b3Vec3 v2 = v[i2]; - - b3Mat33 I; I.SetIdentity(); - - b3Vec3 ef; ef.SetZero(); - - if (m_ks > 0.0f) - { - b3Vec3 dx = x1 - x2; - - float32 L = b3Length(dx); - - if (L >= m_L0) - { - // Apply tension - b3Vec3 n = dx / L; - - ef += -m_ks * (L - m_L0) * n; - - // Jacobian - b3Mat33 Jx11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); - b3Mat33 Jx12 = -Jx11; - //b3Mat33 Jx21 = Jx12; - b3Mat33 Jx22 = Jx11; - - dfdx(i1, i1) += Jx11; - dfdx(i1, i2) += Jx12; - //dfdx(i2, i1) += Jx21; - dfdx(i2, i2) += Jx22; - } - } - - if (m_kd > 0.0f) - { - // Apply damping - b3Vec3 dv = v1 - v2; - - ef += -m_kd * dv; - - b3Mat33 Jv11 = -m_kd * I; - b3Mat33 Jv12 = -Jv11; - //b3Mat33 Jv21 = Jv12; - b3Mat33 Jv22 = Jv11; - - dfdv(i1, i1) += Jv11; - dfdv(i1, i2) += Jv12; - //dfdv(i2, i1) += Jv21; - dfdv(i2, i2) += Jv22; - } - - f[i1] += ef; - f[i2] -= ef; -} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp deleted file mode 100644 index 95632c0..0000000 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ /dev/null @@ -1,1023 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -static B3_FORCE_INLINE u32 b3NextIndex(u32 i) -{ - return i + 1 < 3 ? i + 1 : 0; -} - -struct b3UniqueEdge -{ - u32 v1, v2; -}; - -static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m) -{ - u32 uniqueCount = 0; - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t1 = m->triangles + i; - u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; - - for (u32 j1 = 0; j1 < 3; ++j1) - { - u32 t1v1 = i1s[j1]; - u32 t1v2 = i1s[b3NextIndex(j1)]; - - bool unique = true; - - for (u32 j = 0; j < uniqueCount; ++j) - { - b3UniqueEdge* ue = uniqueEdges + j; - - if (ue->v1 == t1v1 && ue->v2 == t1v2) - { - unique = false; - break; - } - - if (ue->v2 == t1v1 && ue->v1 == t1v2) - { - unique = false; - break; - } - } - - if (unique) - { - b3UniqueEdge ue; - ue.v1 = t1v1; - ue.v2 = t1v2; - uniqueEdges[uniqueCount++] = ue; - } - } - } - - return uniqueCount; -} - -struct b3SharedEdge -{ - u32 v1, v2; - u32 nsv1, nsv2; -}; - -static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) -{ - u32 sharedCount = 0; - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t1 = m->triangles + i; - u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; - - for (u32 j1 = 0; j1 < 3; ++j1) - { - u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0; - - u32 t1v1 = i1s[j1]; - u32 t1v2 = i1s[k1]; - - for (u32 j = i + 1; j < m->triangleCount; ++j) - { - b3ClothMeshTriangle* t2 = m->triangles + j; - u32 i2s[3] = { t2->v1, t2->v2, t2->v3 }; - - for (u32 j2 = 0; j2 < 3; ++j2) - { - u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0; - - u32 t2v1 = i2s[j2]; - u32 t2v2 = i2s[k2]; - - if (t1v1 == t2v2 && t1v2 == t2v1) - { - // The triangles are adjacent. - u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0; - u32 t1v3 = i1s[k3]; - - u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0; - u32 t2v3 = i2s[k4]; - - // Add shared edge and non-shared vertices. - b3SharedEdge se; - se.v1 = t1v1; - se.v2 = t1v2; - se.nsv1 = t1v3; - se.nsv2 = t2v3; - - sharedEdges[sharedCount++] = se; - - break; - } - } - } - } - } - - return sharedCount; -} - -b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : - m_particleBlocks(sizeof(b3Particle)), - m_bodyContactBlocks(sizeof(b3BodyContact)), - m_particleContactBlocks(sizeof(b3ParticleContact)), - m_triangleContactBlocks(sizeof(b3TriangleContact)) -{ - B3_ASSERT(def.mesh); - B3_ASSERT(def.density > 0.0f); - - m_world = world; - m_mesh = def.mesh; - m_density = def.density; - - const b3ClothMesh* m = m_mesh; - - m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); - - // Create particles - for (u32 i = 0; i < m->vertexCount; ++i) - { - b3ParticleDef pd; - pd.type = e_dynamicParticle; - pd.position = m->vertices[i]; - - b3Particle* p = CreateParticle(pd); - - p->m_vertex = i; - - m_vertexParticles[i] = p; - } - - // Compute mass - ComputeMass(); - - // Create forces - b3StackAllocator* allocator = &m_world->m_stackAllocator; - - // Worst-case edge memory - u32 edgeCount = 3 * m->triangleCount; - - b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)allocator->Allocate(edgeCount * sizeof(b3UniqueEdge)); - u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m); - - b3SharedEdge* sharedEdges = (b3SharedEdge*)allocator->Allocate(edgeCount * sizeof(b3SharedEdge)); - u32 sharedCount = b3FindSharedEdges(sharedEdges, m); - - for (u32 i = 0; i < uniqueCount; ++i) - { - b3UniqueEdge* e = uniqueEdges + i; - - b3Particle* p1 = m_vertexParticles[e->v1]; - b3Particle* p2 = m_vertexParticles[e->v2]; - - b3SpringForceDef fd; - fd.Initialize(p1, p2, def.structural, def.damping); - - CreateForce(fd); - } - - // Bending - for (u32 i = 0; i < sharedCount; ++i) - { - b3SharedEdge* e = sharedEdges + i; - - b3Particle* p1 = m_vertexParticles[e->v1]; - b3Particle* p2 = m_vertexParticles[e->v2]; - b3Particle* p3 = m_vertexParticles[e->nsv1]; - b3Particle* p4 = m_vertexParticles[e->nsv2]; - - b3BendForceDef fd; - fd.Initialize(p1, p2, p3, p4, def.bending, def.damping); - - CreateForce(fd); - } - - allocator->Free(sharedEdges); - allocator->Free(uniqueEdges); - - // Sewing - for (u32 i = 0; i < m->sewingLineCount; ++i) - { - b3ClothMeshSewingLine* line = m->sewingLines + i; - - b3Particle* p1 = m_vertexParticles[line->v1]; - b3Particle* p2 = m_vertexParticles[line->v2]; - - b3SpringForceDef fd; - fd.Initialize(p1, p2, def.structural, def.damping); - - CreateForce(fd); - } -} - -b3Cloth::~b3Cloth() -{ - b3Particle* p = m_particleList.m_head; - while (p) - { - b3Particle* p0 = p; - p = p->m_next; - p0->~b3Particle(); - } - - b3Free(m_vertexParticles); - - b3Force* f = m_forceList.m_head; - while (f) - { - b3Force* f0 = f; - f = f->m_next; - f0->~b3Force(); - b3Free(f0); - } -} - -b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def) -{ - void* mem = m_particleBlocks.Allocate(); - b3Particle* p = new(mem) b3Particle(def, this); - m_particleList.PushFront(p); - return p; -} - -void b3Cloth::DestroyParticle(b3Particle* particle) -{ - if (particle->m_vertex != ~0) - { - m_vertexParticles[particle->m_vertex] = NULL; - } - - m_particleList.Remove(particle); - particle->~b3Particle(); - m_particleBlocks.Free(particle); -} - -b3Force* b3Cloth::CreateForce(const b3ForceDef& def) -{ - b3Force* f = b3Force::Create(&def); - m_forceList.PushFront(f); - return f; -} - -void b3Cloth::DestroyForce(b3Force* force) -{ - m_forceList.Remove(force); - b3Force::Destroy(force); -} - -float32 b3Cloth::GetEnergy() const -{ - float32 E = 0.0f; - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - E += p->m_mass * b3Dot(p->m_velocity, p->m_velocity); - } - return 0.5f * E; -} - -b3Particle* b3Cloth::GetVertexParticle(u32 i) -{ - B3_ASSERT(i < m_mesh->vertexCount); - return m_vertexParticles[i]; -} - -void b3Cloth::ComputeMass() -{ - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - p->m_mass = 0.0f; - p->m_invMass = 0.0f; - } - - const float32 inv3 = 1.0f / 3.0f; - const float32 rho = m_density; - - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3ClothMeshTriangle* triangle = m_mesh->triangles + i; - - b3Vec3 v1 = m_mesh->vertices[triangle->v1]; - b3Vec3 v2 = m_mesh->vertices[triangle->v2]; - b3Vec3 v3 = m_mesh->vertices[triangle->v3]; - - float32 area = b3Area(v1, v2, v3); - B3_ASSERT(area > B3_EPSILON); - - float32 mass = rho * area; - - b3Particle* p1 = m_vertexParticles[triangle->v1]; - b3Particle* p2 = m_vertexParticles[triangle->v2]; - b3Particle* p3 = m_vertexParticles[triangle->v3]; - - p1->m_mass += inv3 * mass; - p2->m_mass += inv3 * mass; - p3->m_mass += inv3 * mass; - } - - // Invert - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - B3_ASSERT(p->m_mass > 0.0f); - p->m_invMass = 1.0f / p->m_mass; - } -} - -void b3Cloth::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) -{ - b3RayCastInput input; - input.p1 = p1; - input.p2 = p2; - input.maxFraction = 1.0f; - - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3RayCastOutput subOutput; - if (RayCast(&subOutput, &input, i)) - { - float32 fraction = subOutput.fraction; - b3Vec3 point = (1.0f - fraction) * input.p1 + fraction * input.p2; - b3Vec3 normal = subOutput.normal; - - float32 newFraction = listener->ReportCloth(this, point, normal, fraction, i); - - if (newFraction == 0.0f) - { - // The client has stopped the query. - return; - } - } - } -} - -bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const -{ - b3RayCastInput input; - input.p1 = p1; - input.p2 = p2; - input.maxFraction = 1.0f; - - u32 triangle0 = ~0; - b3RayCastOutput output0; - output0.fraction = B3_MAX_FLOAT; - - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3RayCastOutput subOutput; - if (RayCast(&subOutput, &input, i)) - { - if (subOutput.fraction < output0.fraction) - { - triangle0 = i; - output0.fraction = subOutput.fraction; - output0.normal = subOutput.normal; - } - } - } - - if (triangle0 != ~0) - { - output->triangle = triangle0; - output->fraction = output0.fraction; - output->normal = output0.normal; - - return true; - } - - return false; -} - -bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const -{ - B3_ASSERT(triangleIndex < m_mesh->triangleCount); - b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex; - - b3Vec3 v1 = m_vertexParticles[triangle->v1]->m_position; - b3Vec3 v2 = m_vertexParticles[triangle->v2]->m_position; - b3Vec3 v3 = m_vertexParticles[triangle->v3]->m_position; - - b3Vec3 p1 = input->p1; - b3Vec3 p2 = input->p2; - float32 maxFraction = input->maxFraction; - b3Vec3 d = p2 - p1; - B3_ASSERT(b3LengthSquared(d) > B3_EPSILON * B3_EPSILON); - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - float32 len = b3Length(n); - if (len <= B3_EPSILON) - { - return false; - } - - B3_ASSERT(len > B3_EPSILON); - n /= len; - - float32 numerator = b3Dot(n, v1 - p1); - float32 denominator = b3Dot(n, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 fraction = numerator / denominator; - - // Is the intersection not on the segment? - if (fraction < 0.0f || maxFraction < fraction) - { - return false; - } - - b3Vec3 Q = p1 + fraction * d; - - b3Vec3 A = v1; - b3Vec3 B = v2; - b3Vec3 C = v3; - - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - // Barycentric coordinates for Q - float32 u = b3Dot(QB_x_QC, AB_x_AC); - float32 v = b3Dot(QC_x_QA, AB_x_AC); - float32 w = b3Dot(QA_x_QB, AB_x_AC); - - // Characteristic length of triangle - const float32 kTol = -B3_EPSILON; - - // Is the intersection on the triangle? - if (u > kTol && v > kTol && w > kTol) - { - output->fraction = fraction; - - // Does the ray start from below or above the triangle? - if (numerator > 0.0f) - { - output->normal = -n; - } - else - { - output->normal = n; - } - - return true; - } - - return false; -} - -void b3Cloth::UpdateBodyContacts() -{ - B3_PROFILE("Update Body Contacts"); - - // Clear buffer - b3BodyContact* c = m_bodyContactList.m_head; - while (c) - { - b3BodyContact* c0 = c; - c = c->m_next; - m_bodyContactList.Remove(c0); - c0->~b3BodyContact(); - m_bodyContactBlocks.Free(c0); - } - - // Create contacts - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - b3Sphere s1; - s1.vertex = p->m_position; - s1.radius = p->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 (p->m_type != e_dynamicParticle && body->GetType() != e_dynamicBody) - { - // At least one body should be kinematic or dynamic. - 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) - { - continue; - } - - // Ensure the the normal points from the particle 1 to shape 2 - b3Shape* shape = bestShape; - b3Body* body = shape->GetBody(); - float32 separation = bestSeparation; - b3Vec3 point = bestPoint; - b3Vec3 normal = -bestNormal; - - b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate(); - c->p1 = p; - c->s2 = shape; - 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(); - - m_bodyContactList.PushFront(c); - } -} - -void b3Cloth::UpdateParticleContacts() -{ - B3_PROFILE("Update Particle Contacts"); - - // Clear buffer - b3ParticleContact* c = m_particleContactList.m_head; - while (c) - { - b3ParticleContact* c0 = c; - c = c->m_next; - m_particleContactList.Remove(c0); - c0->~b3ParticleContact(); - m_particleContactBlocks.Free(c0); - } - - // Create particle contacts - for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next) - { - for (b3Particle* p2 = p1->m_next; p2; p2 = p2->m_next) - { - if (p1->m_type != e_dynamicParticle && p2->m_type != e_dynamicBody) - { - // At least one particle should be kinematic or dynamic. - continue; - } - - b3Vec3 c1 = p1->m_position; - float32 r1 = p1->m_radius; - - b3Vec3 c2 = p2->m_position; - float32 r2 = p2->m_radius; - - b3Vec3 d = c2 - c1; - float32 dd = b3Dot(d, d); - float32 totalRadius = r1 + r2; - if (dd > totalRadius * totalRadius) - { - continue; - } - - b3Vec3 n(0.0f, 1.0f, 0.0f); - if (dd > B3_EPSILON * B3_EPSILON) - { - float32 distance = b3Sqrt(dd); - n = d / distance; - } - - b3ParticleContact* c = (b3ParticleContact*)m_particleContactBlocks.Allocate(); - c->p1 = p1; - c->p2 = p2; - c->normalImpulse = 0.0f; - c->t1 = b3Perp(n); - c->t2 = b3Cross(c->t1, n); - c->tangentImpulse.SetZero(); - - m_particleContactList.PushFront(c); - } - } -} - -static B3_FORCE_INLINE void b3Barycentric(float32 out[3], - const b3Vec3& A, const b3Vec3& B, - const b3Vec3& Q) -{ - b3Vec3 AB = B - A; - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - - //float32 divisor = b3Dot(AB, AB); - - out[0] = b3Dot(QB, AB); - out[1] = -b3Dot(QA, AB); - out[2] = out[0] + out[1]; -} - -// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w) -// with respect to a triangle ABC. -// The last output value is the divisor. -static B3_FORCE_INLINE void b3Barycentric(float32 out[4], - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, - const b3Vec3& Q) -{ - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - //float32 divisor = b3Dot(AB_x_AC, AB_x_AC); - - out[0] = b3Dot(QB_x_QC, AB_x_AC); - out[1] = b3Dot(QC_x_QA, AB_x_AC); - out[2] = b3Dot(QA_x_QB, AB_x_AC); - out[3] = out[0] + out[1] + out[2]; -} - -static B3_FORCE_INLINE void b3Solve3(float32 out[3], - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, - const b3Vec3& Q) -{ - // Test vertex regions - float32 wAB[3], wBC[3], wCA[3]; - b3Barycentric(wAB, A, B, Q); - b3Barycentric(wBC, B, C, Q); - b3Barycentric(wCA, C, A, Q); - - // R A - if (wAB[1] <= 0.0f && wCA[0] <= 0.0f) - { - out[0] = 1.0f; - out[1] = 0.0f; - out[2] = 0.0f; - return; - } - - // R B - if (wAB[0] <= 0.0f && wBC[1] <= 0.0f) - { - out[0] = 0.0f; - out[1] = 1.0f; - out[2] = 0.0f; - return; - } - - // R C - if (wBC[0] <= 0.0f && wCA[1] <= 0.0f) - { - out[0] = 0.0f; - out[1] = 0.0f; - out[2] = 1.0f; - return; - } - - // Test edge regions - float32 wABC[4]; - b3Barycentric(wABC, A, B, C, Q); - - // This is used to help testing if the face degenerates - // into an edge. - float32 area = wABC[3]; - - // R AB - if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f) - { - float32 divisor = wAB[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = s * wAB[0]; - out[1] = s * wAB[1]; - out[2] = 0.0f; - return; - } - - // R BC - if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f) - { - float32 divisor = wBC[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = 0.0f; - out[1] = s * wBC[0]; - out[2] = s * wBC[1]; - return; - } - - // R CA - if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f) - { - float32 divisor = wCA[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = 0.0f; - out[2] = s * wCA[0]; - out[1] = s * wCA[1]; - return; - } - - // R ABC/ACB - float32 divisor = wABC[3]; - if (divisor <= 0.0f) - { - float32 s = 1.0f / 3.0f; - out[0] = s; - out[2] = s; - out[1] = s; - return; - } - - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = s * wABC[0]; - out[1] = s * wABC[1]; - out[2] = s * wABC[2]; -} - -void b3Cloth::UpdateTriangleContacts() -{ - B3_PROFILE("Update Triangle Contacts"); - - // Clear buffer - b3TriangleContact* c = m_triangleContactList.m_head; - while (c) - { - b3TriangleContact* c0 = c; - c = c->m_next; - m_triangleContactList.Remove(c0); - c0->~b3TriangleContact(); - m_triangleContactBlocks.Free(c0); - } - - // Create triangle contacts - for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next) - { - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3ClothMeshTriangle* triangle = m_mesh->triangles + i; - - b3Particle* p2 = m_vertexParticles[triangle->v1]; - b3Particle* p3 = m_vertexParticles[triangle->v2]; - b3Particle* p4 = m_vertexParticles[triangle->v3]; - - if (p1 == p2 || p1 == p3 || p1 == p4) - { - continue; - } - - float32 r1 = p1->m_radius; - float32 r2 = 0.0f; - - float32 totalRadius = r1 + r2; - - b3Vec3 A = p2->m_position; - b3Vec3 B = p3->m_position; - b3Vec3 C = p4->m_position; - - b3Vec3 P = p1->m_position; - - b3Vec3 n = b3Cross(B - A, C - A); - float32 n_len = n.Normalize(); - - float32 distance = b3Dot(n, P - A); - - if (distance < -totalRadius) - { - continue; - } - - if (distance > totalRadius) - { - continue; - } - - // Project particle on triangle plane - b3Vec3 Q = P - distance * n; - - // Barycentric coordinates for Q - float32 wABC[3]; - b3Solve3(wABC, A, B, C, Q); - - b3Vec3 c1 = p1->m_position; - b3Vec3 c2 = wABC[0] * A + wABC[1] * B + wABC[2] * C; - - if (b3DistanceSquared(c1, c2) > totalRadius * totalRadius) - { - continue; - } - - bool front = false; - - // Is the the other point in front of the triangle plane? - if (distance >= 0.0f) - { - front = true; - } - - b3TriangleContact* c = (b3TriangleContact*)m_triangleContactBlocks.Allocate(); - c->p1 = p1; - c->p2 = p2; - c->p3 = p3; - c->p4 = p4; - c->front = front; - c->normalImpulse = 0.0f; - - m_triangleContactList.PushFront(c); - } - } -} - -void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) -{ - B3_PROFILE("Solve"); - - // Solve - b3ClothSolverDef solverDef; - solverDef.stack = &m_world->m_stackAllocator; - solverDef.particleCapacity = m_particleList.m_count; - solverDef.forceCapacity = m_forceList.m_count; - solverDef.bodyContactCapacity = m_bodyContactList.m_count; - solverDef.particleContactCapacity = m_particleContactList.m_count; - solverDef.triangleContactCapacity = m_triangleContactList.m_count; - - b3ClothSolver solver(solverDef); - - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - solver.Add(p); - } - - for (b3Force* f = m_forceList.m_head; f; f = f->m_next) - { - solver.Add(f); - } - - for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next) - { - solver.Add(c); - } - - for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next) - { - solver.Add(c); - } - - for (b3TriangleContact* c = m_triangleContactList.m_head; c; c = c->m_next) - { - solver.Add(c); - } - - // Solve - solver.Solve(dt, gravity); - - // Clear external applied forces and translations - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - p->m_force.SetZero(); - p->m_translation.SetZero(); - } -} - -void b3Cloth::UpdateContacts() -{ - // Update body contacts - UpdateBodyContacts(); - - // Update particle contacts - UpdateParticleContacts(); - - // Update triangle contacts - UpdateTriangleContacts(); -} - -void b3Cloth::Step(float32 dt, const b3Vec3& gravity) -{ - B3_PROFILE("Step"); - - // Update contacts - UpdateContacts(); - - // Solve constraints, integrate state, clear forces and translations. - if (dt > 0.0f) - { - Solve(dt, gravity); - } -} - -void b3Cloth::Draw() const -{ - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - if (p->m_type == e_staticParticle) - { - b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_white); - } - - if (p->m_type == e_kinematicParticle) - { - b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_blue); - } - - if (p->m_type == e_dynamicParticle) - { - b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green); - } - } - - for (b3Force* f = m_forceList.m_head; f; f = f->m_next) - { - if (f->m_type == e_springForce) - { - b3SpringForce* s = (b3SpringForce*)f; - b3Particle* p1 = s->m_p1; - b3Particle* p2 = s->m_p2; - - b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_black); - } - } - - const b3ClothMesh* m = m_mesh; - - for (u32 i = 0; i < m->sewingLineCount; ++i) - { - b3ClothMeshSewingLine* s = m->sewingLines + i; - b3Particle* p1 = m_vertexParticles[s->v1]; - b3Particle* p2 = m_vertexParticles[s->v2]; - - b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_white); - } - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t = m->triangles + i; - - b3Particle* p1 = m_vertexParticles[t->v1]; - b3Particle* p2 = m_vertexParticles[t->v2]; - b3Particle* p3 = m_vertexParticles[t->v3]; - - b3Vec3 v1 = p1->m_position; - b3Vec3 v2 = p2->m_position; - b3Vec3 v3 = p3->m_position; - - b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); - n1.Normalize(); - b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue); - - b3Vec3 n2 = -n1; - b3Draw_draw->DrawSolidTriangle(n2, v1, v3, v2, b3Color_blue); - } -} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp deleted file mode 100644 index 763031f..0000000 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ /dev/null @@ -1,465 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. -// described in the paper: -// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". - -// Some improvements for the original MPCG algorithm are described in the paper: -// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". - -u32 b3_clothSolverIterations = 0; - -b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) -{ - m_allocator = def.stack; - - m_particleCapacity = def.particleCapacity; - m_particleCount = 0; - m_particles = (b3Particle**)m_allocator->Allocate(m_particleCapacity * sizeof(b3Particle*)); - - m_forceCapacity = def.forceCapacity; - m_forceCount = 0; - m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));; - - m_constraintCapacity = def.particleCapacity; - m_constraintCount = 0; - m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); - - m_bodyContactCapacity = def.bodyContactCapacity; - m_bodyContactCount = 0; - m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));; - - m_particleContactCapacity = def.particleContactCapacity; - m_particleContactCount = 0; - m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*));; - - m_triangleContactCapacity = def.triangleContactCapacity; - m_triangleContactCount = 0; - m_triangleContacts = (b3TriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3TriangleContact*));; -} - -b3ClothSolver::~b3ClothSolver() -{ - m_allocator->Free(m_triangleContacts); - m_allocator->Free(m_particleContacts); - m_allocator->Free(m_bodyContacts); - - m_allocator->Free(m_constraints); - m_allocator->Free(m_forces); - m_allocator->Free(m_particles); -} - -void b3ClothSolver::Add(b3Particle* p) -{ - p->m_solverId = m_particleCount; - m_particles[m_particleCount++] = p; -} - -void b3ClothSolver::Add(b3Force* f) -{ - m_forces[m_forceCount++] = f; -} - -void b3ClothSolver::Add(b3BodyContact* c) -{ - m_bodyContacts[m_bodyContactCount++] = c; -} - -void b3ClothSolver::Add(b3ParticleContact* c) -{ - m_particleContacts[m_particleContactCount++] = c; -} - -void b3ClothSolver::Add(b3TriangleContact* c) -{ - m_triangleContacts[m_triangleContactCount++] = c; -} - -void b3ClothSolver::ApplyForces() -{ - for (u32 i = 0; i < m_forceCount; ++i) - { - m_forces[i]->Apply(&m_solverData); - } -} - -void b3AccelerationConstraint::Apply(const b3ClothSolverData* data) -{ - b3DiagMat33& sS = *data->S; - b3DenseVec3& sz = *data->z; - - sz[i1] = z; - - b3Mat33 I; I.SetIdentity(); - - switch (ndof) - { - case 3: - { - sS[i1] = I; - break; - } - case 2: - { - sS[i1] = I - b3Outer(p, p); - break; - } - case 1: - { - sS[i1] = I - b3Outer(p, p) - b3Outer(q, q); - break; - } - case 0: - { - sS[i1].SetZero(); - break; - } - default: - { - B3_ASSERT(false); - break; - } - } -} - -void b3ClothSolver::ApplyConstraints() -{ - b3DiagMat33& S = *m_solverData.S; - b3DenseVec3& z = *m_solverData.z; - - S.SetIdentity(); - z.SetZero(); - - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - if (p->m_type != e_dynamicParticle) - { - b3AccelerationConstraint* ac = m_constraints + m_constraintCount; - ++m_constraintCount; - ac->i1 = i; - ac->ndof = 0; - ac->z.SetZero(); - } - } - - for (u32 i = 0; i < m_constraintCount; ++i) - { - m_constraints[i].Apply(&m_solverData); - } -} - -void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) -{ - float32 h = dt; - float32 inv_h = 1.0f / h; - - b3DenseVec3 sx(m_particleCount); - b3DenseVec3 sv(m_particleCount); - b3DenseVec3 sf(m_particleCount); - - m_solverData.dt = h; - m_solverData.invdt = inv_h; - m_solverData.x = &sx; - m_solverData.v = &sv; - m_solverData.f = &sf; - - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - sx[i] = p->m_position; - sv[i] = p->m_velocity; - sf[i] = p->m_force; - - // Apply weight - if (p->m_type == e_dynamicParticle) - { - sf[i] += p->m_mass * gravity; - } - } - - // Integrate velocities - { - b3SparseSymMat33 dfdx(m_particleCount); - b3SparseSymMat33 dfdv(m_particleCount); - b3DiagMat33 S(m_particleCount); - b3DenseVec3 z(m_particleCount); - b3DenseVec3 sy(m_particleCount); - b3DenseVec3 sx0(m_particleCount); - - m_solverData.y = &sy; - m_solverData.dfdx = &dfdx; - m_solverData.dfdv = &dfdv; - m_solverData.S = &S; - m_solverData.z = &z; - - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - sy[i] = p->m_translation; - sx0[i] = p->m_x; - } - - // Apply internal forces - ApplyForces(); - - // Apply constraints - ApplyConstraints(); - - // Solve Ax = b, where - // A = M - h * dfdv - h * h * dfdx - // b = h * (f0 + h * dfdx * v0 + dfdx * y) - - b3SparseSymMat33 M(m_particleCount); - for (u32 i = 0; i < m_particleCount; ++i) - { - M(i, i) = b3Diagonal(m_particles[i]->m_mass); - } - - // A - b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx; - - // b - b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); - - // x - b3DenseVec3 x(m_particleCount); - u32 iterations = 0; - Solve(x, iterations, A, b, S, z, sx0); - b3_clothSolverIterations = iterations; - - sv = sv + x; - sx = sx + sy; - - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - p->m_x = x[i]; - p->m_position = sx[i]; - p->m_velocity = sv[i]; - } - } - - // Solve constraints - b3ClothContactSolverDef contactSolverDef; - contactSolverDef.allocator = m_allocator; - contactSolverDef.positions = m_solverData.x; - contactSolverDef.velocities = m_solverData.v; - contactSolverDef.bodyContactCount = m_bodyContactCount; - contactSolverDef.bodyContacts = m_bodyContacts; - contactSolverDef.particleContactCount = m_particleContactCount; - contactSolverDef.particleContacts = m_particleContacts; - contactSolverDef.triangleContactCount = m_triangleContactCount; - contactSolverDef.triangleContacts = m_triangleContacts; - - b3ClothContactSolver contactSolver(contactSolverDef); - - { - contactSolver.InitializeBodyContactConstraints(); - contactSolver.InitializeParticleContactConstraints(); - contactSolver.InitializeTriangleContactConstraints(); - } - - { - contactSolver.WarmStart(); - } - - // Solve velocity constraints - { - const u32 kVelocityIterations = 5; - - for (u32 i = 0; i < kVelocityIterations; ++i) - { - contactSolver.SolveBodyContactVelocityConstraints(); - contactSolver.SolveParticleContactVelocityConstraints(); - contactSolver.SolveTriangleContactVelocityConstraints(); - } - } - - { - contactSolver.StoreImpulses(); - } - - // Integrate positions - sx = sx + h * sv; - - // Solve position constraints - { - const u32 kPositionIterations = 2; - - bool positionSolved = false; - for (u32 i = 0; i < kPositionIterations; ++i) - { - bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); - bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints(); - bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints(); - - if (bodyContactsSolved && particleContactsSolved && triangleContactsSolved) - { - positionSolved = true; - break; - } - } - } - - // Synchronize bodies - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3Body* body = m_bodyContacts[i]->s2->GetBody(); - - body->SynchronizeTransform(); - - body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation); - - body->SynchronizeShapes(); - } - - // Copy state buffers back to the particles - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - p->m_position = sx[i]; - p->m_velocity = sv[i]; - } -} - -void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, - const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const -{ - B3_PROFILE("Solve Ax = b"); - - // P = diag(A) - b3DiagMat33 inv_P(m_particleCount); - A.Diagonal(inv_P); - - // Invert - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Mat33& D = inv_P[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; - - D = b3Diagonal(xx, yy, zz); - } - - // I - b3DiagMat33 I(m_particleCount); - I.SetIdentity(); - - // x = S * y + (I - S) * z - x = (S * y) + (I - S) * z; - - // b^ = S * (b - A * (I - S) * z) - b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); - - // b_delta = dot(b^, P^-1 * b_^) - float32 b_delta = b3Dot(b_hat, inv_P * b_hat); - - // r = S * (b - A * x) - b3DenseVec3 r = S * (b - A * x); - - // p = S * (P^-1 * r) - b3DenseVec3 p = S * (inv_P * r); - - // delta_new = dot(r, p) - float32 delta_new = b3Dot(r, p); - - // Set the tolerance. - const float32 tolerance = 10.0f * B3_EPSILON; - - // Maximum number of iterations. - // Stop at this iteration if diverged. - const u32 max_iterations = 20; - - u32 iteration = 0; - - // Main iteration loop. - for (;;) - { - // Divergence check. - if (iteration >= max_iterations) - { - break; - } - - // Convergence check. - if (delta_new <= tolerance * tolerance * b_delta) - { - break; - } - - // s = S * (A * p) - b3DenseVec3 s = S * (A * p); - - // alpha = delta_new / dot(p, s) - float32 alpha = delta_new / b3Dot(p, s); - - // x = x + alpha * p - x = x + alpha * p; - - // r = r - alpha * s - r = r - alpha * s; - - // h = inv_P * r - b3DenseVec3 h = inv_P * r; - - // delta_old = delta_new - float32 delta_old = delta_new; - - // delta_new = dot(r, h) - delta_new = b3Dot(r, h); - - // beta = delta_new / delta_old - float32 beta = delta_new / delta_old; - - // p = S * (h + beta * p) - p = S * (h + beta * p); - - ++iteration; - } - - iterations = iteration; -} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp deleted file mode 100644 index cd4c2fe..0000000 --- a/src/bounce/dynamics/cloth/particle.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) -{ - b3Vec3 cA = b3Mul(xfA, c->localPoint1); - b3Vec3 cB = b3Mul(xfB, c->localPoint2); - - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = distance - rA - rB; -} - -void b3ParticleContactWorldPoint::Initialize(const b3ParticleContact* c) -{ - b3Vec3 cA = c->p1->GetPosition(); - float32 rA = c->p1->GetRadius(); - - b3Vec3 cB = c->p2->GetPosition(); - float32 rB = c->p2->GetRadius(); - - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = distance - rA - rB; -} - -b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) -{ - m_cloth = cloth; - m_type = def.type; - - m_position = def.position; - m_velocity = def.velocity; - m_force = def.force; - m_translation.SetZero(); - m_mass = 0.0f; - m_invMass = 0.0f; - m_radius = def.radius; - m_friction = def.friction; - m_userData = nullptr; - m_x.SetZero(); - m_vertex = ~0; -} - -b3Particle::~b3Particle() -{ - -} - -void b3Particle::SetType(b3ParticleType type) -{ - if (m_type == type) - { - return; - } - - m_type = type; - m_force.SetZero(); - - if (type == e_staticParticle) - { - m_velocity.SetZero(); - m_translation.SetZero(); - } -} \ No newline at end of file diff --git a/src/bounce/dynamics/contact_manager.cpp b/src/bounce/dynamics/contact_manager.cpp index b6d5ddc..715298d 100644 --- a/src/bounce/dynamics/contact_manager.cpp +++ b/src/bounce/dynamics/contact_manager.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/collide/clip.cpp b/src/bounce/dynamics/contacts/collide/clip.cpp index ded4e26..78be3c8 100644 --- a/src/bounce/dynamics/contacts/collide/clip.cpp +++ b/src/bounce/dynamics/contacts/collide/clip.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/collide/collide.cpp b/src/bounce/dynamics/contacts/collide/collide.cpp index 9292145..3e716ec 100644 --- a/src/bounce/dynamics/contacts/collide/collide.cpp +++ b/src/bounce/dynamics/contacts/collide/collide.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp index 8a5d586..fef0862 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -52,7 +52,7 @@ static void b3BuildEdgeContact(b3Manifold& manifold, // Compute the closest points on the two lines. float32 b = b3Dot(N1, N2); float32 den = 1.0f - b * b; - if (den <= 0.0f) + if (den == 0.0f) { return; } diff --git a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp index 7f1a558..51d70c2 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -24,7 +24,7 @@ #include // Compute the closest point on a segment to a point. -static b3Vec3 b3ClosestPoint(const b3Vec3& Q, const b3Capsule& hull) +static b3Vec3 b3ClosestPointOnSegment(const b3Vec3& Q, const b3Capsule& hull) { b3Vec3 A = hull.vertices[0]; b3Vec3 B = hull.vertices[1]; @@ -81,13 +81,13 @@ static void b3ClosestPoints(b3Vec3& C1, b3Vec3& C2, if (L1 < B3_LINEAR_SLOP) { C1 = P1; - C2 = b3ClosestPoint(P1, hull2); + C2 = b3ClosestPointOnSegment(P1, hull2); return; } if (L2 < B3_LINEAR_SLOP) { - C1 = b3ClosestPoint(P2, hull1); + C1 = b3ClosestPointOnSegment(P2, hull1); C2 = P2; return; } @@ -144,11 +144,11 @@ static void b3ClosestPoints(b3Vec3& C1, b3Vec3& C2, C2 = P2; } - C1 = b3ClosestPoint(C1, hull1); + C1 = b3ClosestPointOnSegment(C1, hull1); - C2 = b3ClosestPoint(C1, hull2); + C2 = b3ClosestPointOnSegment(C1, hull2); - C1 = b3ClosestPoint(C2, hull1); + C1 = b3ClosestPointOnSegment(C2, hull1); } static bool b3AreParalell(const b3Capsule& hull1, const b3Capsule& hull2) @@ -211,8 +211,8 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold, if (clipCount == 2) { - b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdge1[0].position, hull2.vertices[0], hull2.vertices[1]); - b3Vec3 cp2 = b3ClosestPointOnSegment(clipEdge1[1].position, hull2.vertices[0], hull2.vertices[1]); + b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdge1[0].position, hull2); + b3Vec3 cp2 = b3ClosestPointOnSegment(clipEdge1[1].position, hull2); float32 d1 = b3Distance(clipEdge1[0].position, cp1); float32 d2 = b3Distance(clipEdge1[1].position, cp2); diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp index 70005e1..63ae512 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -54,7 +54,7 @@ void b3BuildEdgeContact(b3Manifold& manifold, // Compute the closest points on the two lines. float32 b = b3Dot(N1, N2); float32 den = 1.0f - b * b; - if (den <= 0.0f) + if (den == 0.0f) { return; } diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index a7cada8..bcdc59e 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -63,7 +63,7 @@ static void b3RebuildEdgeContact(b3Manifold& manifold, // Compute the closest points on the two lines. float32 b = b3Dot(N1, N2); float32 den = 1.0f - b * b; - if (den <= 0.0f) + if (den == 0.0f) { return; } diff --git a/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp b/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp index ae8e843..0ffc545 100644 --- a/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_sphere_capsule.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp index 5b425a0..15ebb47 100644 --- a/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_sphere_hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/collide/collide_spheres.cpp b/src/bounce/dynamics/contacts/collide/collide_spheres.cpp index 3826736..66d8c4f 100644 --- a/src/bounce/dynamics/contacts/collide/collide_spheres.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_spheres.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/contact.cpp b/src/bounce/dynamics/contacts/contact.cpp index 973ad49..82da917 100644 --- a/src/bounce/dynamics/contacts/contact.cpp +++ b/src/bounce/dynamics/contacts/contact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/contact_cluster.cpp b/src/bounce/dynamics/contacts/contact_cluster.cpp index 1c4be98..bdc4109 100644 --- a/src/bounce/dynamics/contacts/contact_cluster.cpp +++ b/src/bounce/dynamics/contacts/contact_cluster.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/contact_solver.cpp b/src/bounce/dynamics/contacts/contact_solver.cpp index ee3d7ec..4d33001 100644 --- a/src/bounce/dynamics/contacts/contact_solver.cpp +++ b/src/bounce/dynamics/contacts/contact_solver.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/convex_contact.cpp b/src/bounce/dynamics/contacts/convex_contact.cpp index 472e997..fd4bc3d 100644 --- a/src/bounce/dynamics/contacts/convex_contact.cpp +++ b/src/bounce/dynamics/contacts/convex_contact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/manifold.cpp b/src/bounce/dynamics/contacts/manifold.cpp index 4164e8c..bbe0380 100644 --- a/src/bounce/dynamics/contacts/manifold.cpp +++ b/src/bounce/dynamics/contacts/manifold.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp index e5a69e7..f3aaf99 100644 --- a/src/bounce/dynamics/contacts/mesh_contact.cpp +++ b/src/bounce/dynamics/contacts/mesh_contact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/draw_world.cpp b/src/bounce/dynamics/draw_world.cpp index 3b674d4..5bd5477 100644 --- a/src/bounce/dynamics/draw_world.cpp +++ b/src/bounce/dynamics/draw_world.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -51,7 +51,7 @@ void b3World::Draw() const const b3Transform& xf = b->GetTransform(); for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next) { - DrawShape(xf, s); + DrawShape(xf, s, b3Color_black); } } } @@ -142,16 +142,15 @@ void b3World::Draw() const } } -void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const +void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape, const b3Color& color) const { - b3Color wireColor(0.0f, 0.0f, 0.0f); switch (shape->GetType()) { case e_sphereShape: { const b3SphereShape* sphere = (b3SphereShape*)shape; b3Vec3 p = xf * sphere->m_center; - b3Draw_draw->DrawPoint(p, 4.0f, wireColor); + b3Draw_draw->DrawPoint(p, 4.0f, color); break; } case e_capsuleShape: @@ -159,9 +158,9 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const const b3CapsuleShape* capsule = (b3CapsuleShape*)shape; b3Vec3 p1 = xf * capsule->m_centers[0]; b3Vec3 p2 = xf * capsule->m_centers[1]; - b3Draw_draw->DrawPoint(p1, 4.0f, wireColor); - b3Draw_draw->DrawPoint(p2, 4.0f, wireColor); - b3Draw_draw->DrawSegment(p1, p2, wireColor); + b3Draw_draw->DrawPoint(p1, 4.0f, color); + b3Draw_draw->DrawPoint(p2, 4.0f, color); + b3Draw_draw->DrawSegment(p1, p2, color); break; } case e_hullShape: @@ -176,7 +175,7 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const b3Vec3 p1 = xf * hull->vertices[edge->origin]; b3Vec3 p2 = xf * hull->vertices[twin->origin]; - b3Draw_draw->DrawSegment(p1, p2, wireColor); + b3Draw_draw->DrawSegment(p1, p2, color); } break; } @@ -192,7 +191,7 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const b3Vec3 p2 = xf * mesh->vertices[t->v2]; b3Vec3 p3 = xf * mesh->vertices[t->v3]; - b3Draw_draw->DrawTriangle(p1, p2, p3, wireColor); + b3Draw_draw->DrawTriangle(p1, p2, p3, color); } break; } @@ -201,4 +200,122 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const break; } }; +} + +void b3World::DrawSolid() const +{ + for (b3Body* b = m_bodyList.m_head; b; b = b->GetNext()) + { + b3Color c; + if (b->IsAwake() == false) + { + c = b3Color(0.5f, 0.25f, 0.25f, 1.0f); + } + else if (b->GetType() == e_staticBody) + { + c = b3Color(0.5f, 0.5f, 0.5f, 1.0f); + } + else if (b->GetType() == e_dynamicBody) + { + c = b3Color(1.0f, 0.5f, 0.5f, 1.0f); + } + else + { + c = b3Color(0.5f, 0.5f, 1.0f, 1.0f); + } + + b3Transform xf = b->GetTransform(); + for (b3Shape* s = b->GetShapeList().m_head; s; s = s->GetNext()) + { + DrawSolidShape(xf, s, c); + } + } +} + +void b3World::DrawSolidShape(const b3Transform& xf, const b3Shape* shape, const b3Color& color) const +{ + switch (shape->GetType()) + { + case e_sphereShape: + { + const b3SphereShape* sphere = (b3SphereShape*)shape; + + b3Vec3 center = xf * sphere->m_center; + + b3Draw_draw->DrawSolidSphere(center, sphere->m_radius, xf.rotation, color); + + break; + } + case e_capsuleShape: + { + const b3CapsuleShape* capsule = (b3CapsuleShape*)shape; + + b3Vec3 c1 = xf * capsule->m_centers[0]; + b3Vec3 c2 = xf * capsule->m_centers[1]; + + b3Draw_draw->DrawSolidCapsule(c1, c2, capsule->m_radius, xf.rotation, color); + + break; + } + case e_hullShape: + { + const b3HullShape* hullShape = (b3HullShape*)shape; + + const b3Hull* hull = hullShape->m_hull; + + for (u32 i = 0; i < hull->faceCount; ++i) + { + const b3Face* face = hull->GetFace(i); + const b3HalfEdge* begin = hull->GetEdge(face->edge); + + b3Vec3 n = xf.rotation * hull->planes[i].normal; + + const b3HalfEdge* edge = hull->GetEdge(begin->next); + do + { + u32 i1 = begin->origin; + u32 i2 = edge->origin; + const b3HalfEdge* next = hull->GetEdge(edge->next); + u32 i3 = next->origin; + + b3Vec3 p1 = xf * hull->vertices[i1]; + b3Vec3 p2 = xf * hull->vertices[i2]; + b3Vec3 p3 = xf * hull->vertices[i3]; + + b3Draw_draw->DrawSolidTriangle(n, p1, p2, p3, color); + + edge = next; + } while (hull->GetEdge(edge->next) != begin); + } + + break; + } + case e_meshShape: + { + const b3MeshShape* meshShape = (b3MeshShape*)shape; + + const b3Mesh* mesh = meshShape->m_mesh; + for (u32 i = 0; i < mesh->triangleCount; ++i) + { + const b3Triangle* t = mesh->triangles + i; + + b3Vec3 p1 = xf * mesh->vertices[t->v1]; + b3Vec3 p2 = xf * mesh->vertices[t->v2]; + b3Vec3 p3 = xf * mesh->vertices[t->v3]; + + b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1); + n1.Normalize(); + b3Draw_draw->DrawSolidTriangle(n1, p1, p2, p3, color); + + b3Vec3 n2 = -n1; + b3Draw_draw->DrawSolidTriangle(n2, p3, p2, p1, color); + } + + break; + } + default: + { + break; + } + }; } \ No newline at end of file diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index 305c66f..d0e831a 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joint_manager.cpp b/src/bounce/dynamics/joint_manager.cpp index b81c579..fb29016 100644 --- a/src/bounce/dynamics/joint_manager.cpp +++ b/src/bounce/dynamics/joint_manager.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/cone_joint.cpp b/src/bounce/dynamics/joints/cone_joint.cpp index 15f41ac..fcf54c2 100644 --- a/src/bounce/dynamics/joints/cone_joint.cpp +++ b/src/bounce/dynamics/joints/cone_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/joint.cpp b/src/bounce/dynamics/joints/joint.cpp index 2d1593c..1f380b7 100644 --- a/src/bounce/dynamics/joints/joint.cpp +++ b/src/bounce/dynamics/joints/joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/joint_solver.cpp b/src/bounce/dynamics/joints/joint_solver.cpp index 234b74a..7336a80 100644 --- a/src/bounce/dynamics/joints/joint_solver.cpp +++ b/src/bounce/dynamics/joints/joint_solver.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/mouse_joint.cpp b/src/bounce/dynamics/joints/mouse_joint.cpp index dc38de1..e816c1f 100644 --- a/src/bounce/dynamics/joints/mouse_joint.cpp +++ b/src/bounce/dynamics/joints/mouse_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp index 07dbec8..f596dbe 100644 --- a/src/bounce/dynamics/joints/revolute_joint.cpp +++ b/src/bounce/dynamics/joints/revolute_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -112,7 +112,7 @@ C' = P_lim * (P_hinge * q') - target_speed */ -static B3_FORCE_INLINE b3Mat44 iQ_mat(const b3Quat& q) +static B3_FORCE_INLINE b3Mat44 b3Mat44_Quat(const b3Quat& q) { b3Mat44 Q; Q.x = b3Vec4(q.w, q.x, q.y, q.z); @@ -122,7 +122,7 @@ static B3_FORCE_INLINE b3Mat44 iQ_mat(const b3Quat& q) return Q; } -static B3_FORCE_INLINE b3Mat44 iP_mat(const b3Quat& q) +static B3_FORCE_INLINE b3Mat44 b3Mat44_Projection(const b3Quat& q) { b3Mat44 P; P.x = b3Vec4(q.w, q.x, q.y, q.z); @@ -132,7 +132,7 @@ static B3_FORCE_INLINE b3Mat44 iP_mat(const b3Quat& q) return P; } -static B3_FORCE_INLINE b3Mat34 P_mat() +static B3_FORCE_INLINE b3Mat34 b3Mat34_Projection() { b3Mat34 P; P.x = b3Vec3(0.0f, 0.0f, 0.0f); @@ -142,7 +142,7 @@ static B3_FORCE_INLINE b3Mat34 P_mat() return P; } -static B3_FORCE_INLINE b3Mat24 P_hinge_mat() +static B3_FORCE_INLINE b3Mat24 b3Mat24_Hinge_Projection() { b3Mat24 P; P.x = b3Vec2(0.0f, 0.0f); @@ -152,21 +152,19 @@ static B3_FORCE_INLINE b3Mat24 P_hinge_mat() return P; } -// 1x4 -static B3_FORCE_INLINE b3Vec4 P_hinge_limit_mat(const b3Quat& q) +static B3_FORCE_INLINE b3Vec4 b3Mat14_Hinge_Limit_Projection(const b3Quat& q) { return b3Vec4(-q.z, 0.0f, 0.0f, q.w); } -// 4x1 -static B3_FORCE_INLINE b3Vec4 q_to_v(const b3Quat& q) +static B3_FORCE_INLINE b3Vec4 b3Vec4_Quat(const b3Quat& q) { return b3Vec4(q.w, q.x, q.y, q.z); } -static const b3Mat34 P = P_mat(); -static const b3Mat43 PT = b3Transpose(P); -static const b3Mat24 P_hinge = P_hinge_mat(); +static const b3Mat34 b3Mat34_P = b3Mat34_Projection(); +static const b3Mat43 b3Mat43_PT = b3Transpose(b3Mat34_P); +static const b3Mat24 b3Mat24_P_Hinge = b3Mat24_Hinge_Projection(); void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& axis, const b3Vec3& anchor, @@ -263,13 +261,14 @@ void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data) // Add motor constraint. if (m_enableMotor || m_enableLimit) { - b3Vec4 P_hinge_limit = P_hinge_limit_mat(q); + b3Mat43 PT = b3Mat43_PT; + b3Vec4 P_limit = b3Mat14_Hinge_Limit_Projection(q); - b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); - b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); + b3Mat44 G1 = -0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); + b3Mat44 G2 = 0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); - b3Vec3 J1 = P_hinge_limit * G1 * PT; - b3Vec3 J2 = P_hinge_limit * G2 * PT; + b3Vec3 J1 = P_limit * G1 * PT; + b3Vec3 J2 = P_limit * G2 * PT; b3Vec3 J1T = J1; b3Vec3 J2T = J2; @@ -338,8 +337,11 @@ void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data) // Add hinge constraints. { - b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); - b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); + b3Mat43 PT = b3Mat43_PT; + b3Mat24 P_hinge = b3Mat24_P_Hinge; + + b3Mat44 G1 = -0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); + b3Mat44 G2 = 0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); b3Mat23 J1 = P_hinge * G1 * PT; b3Mat23 J2 = P_hinge * G2 * PT; @@ -518,13 +520,14 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat fB = qB * m_localRotationB; b3Quat q = b3Conjugate(m_referenceRotation) * b3Conjugate(fA) * fB; - b3Vec4 P_hinge_limit = P_hinge_limit_mat(q); + b3Mat43 PT = b3Mat43_PT; + b3Vec4 P_limit = b3Mat14_Hinge_Limit_Projection(q); + + b3Mat44 G1 = -0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); + b3Mat44 G2 = 0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); - b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); - b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); - - b3Vec3 J1 = P_hinge_limit * G1 * PT; - b3Vec3 J2 = P_hinge_limit * G2 * PT; + b3Vec3 J1 = P_limit * G1 * PT; + b3Vec3 J2 = P_limit * G2 * PT; b3Vec3 J1T = J1; b3Vec3 J2T = J2; @@ -617,13 +620,16 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat fB = qB * m_localRotationB; b3Quat q = b3Conjugate(m_referenceRotation) * b3Conjugate(fA) * fB; - b3Vec2 C = P_hinge * q_to_v(q); + b3Mat43 PT = b3Mat43_PT; + b3Mat24 P_hinge = b3Mat24_P_Hinge; + + b3Vec2 C = P_hinge * b3Vec4_Quat(q); angularError += b3Length(C); // Compute effective mass - b3Mat44 G1 = -0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); - b3Mat44 G2 = 0.5f * iQ_mat(b3Conjugate(fA)) * iP_mat(fB); + b3Mat44 G1 = -0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); + b3Mat44 G2 = 0.5f * b3Mat44_Quat(b3Conjugate(fA)) * b3Mat44_Projection(fB); b3Mat23 J1 = P_hinge * G1 * PT; b3Mat23 J2 = P_hinge * G2 * PT; diff --git a/src/bounce/dynamics/joints/sphere_joint.cpp b/src/bounce/dynamics/joints/sphere_joint.cpp index ecbdd16..72bc346 100644 --- a/src/bounce/dynamics/joints/sphere_joint.cpp +++ b/src/bounce/dynamics/joints/sphere_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/spring_joint.cpp b/src/bounce/dynamics/joints/spring_joint.cpp index 93d2541..8ce5759 100644 --- a/src/bounce/dynamics/joints/spring_joint.cpp +++ b/src/bounce/dynamics/joints/spring_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/joints/weld_joint.cpp b/src/bounce/dynamics/joints/weld_joint.cpp index 22e15b0..e9bd161 100644 --- a/src/bounce/dynamics/joints/weld_joint.cpp +++ b/src/bounce/dynamics/joints/weld_joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -41,7 +41,7 @@ J1 = P * J1 * P^T J2 = P * J2 * P^T */ -static b3Mat44 iQ_mat(const b3Quat& q) +static B3_FORCE_INLINE b3Mat44 b3Mat44_Quat(const b3Quat& q) { b3Mat44 Q; Q.x = b3Vec4(q.w, q.x, q.y, q.z); @@ -51,7 +51,7 @@ static b3Mat44 iQ_mat(const b3Quat& q) return Q; } -static b3Mat44 iP_mat(const b3Quat& q) +static B3_FORCE_INLINE b3Mat44 b3Mat44_Projection(const b3Quat& q) { b3Mat44 P; P.x = b3Vec4(q.w, q.x, q.y, q.z); @@ -61,7 +61,7 @@ static b3Mat44 iP_mat(const b3Quat& q) return P; } -static b3Mat34 P_mat() +static B3_FORCE_INLINE b3Mat34 b3Mat34_Projection() { b3Mat34 P; P.x = b3Vec3(0.0f, 0.0f, 0.0f); @@ -71,7 +71,7 @@ static b3Mat34 P_mat() return P; } -static b3Mat34 P_lock_mat() +static B3_FORCE_INLINE b3Mat34 b3Mat34_Weld_Projection() { b3Mat34 P; P.x = b3Vec3(0.0f, 0.0f, 0.0f); @@ -81,14 +81,14 @@ static b3Mat34 P_lock_mat() return P; } -static b3Vec4 q_to_v(const b3Quat& q) +static B3_FORCE_INLINE b3Vec4 b3Vec4_Quat(const b3Quat& q) { return b3Vec4(q.w, q.x, q.y, q.z); } -static const b3Mat34 P = P_mat(); -static const b3Mat43 PT = b3Transpose(P); -static const b3Mat34 P_lock = P_lock_mat(); +static const b3Mat34 b3Mat34_P = b3Mat34_Projection(); +static const b3Mat43 b3Mat43_PT = b3Transpose(b3Mat34_P); +static const b3Mat34 b3Mat34_Weld_P = b3Mat34_Weld_Projection(); void b3WeldJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor) { @@ -147,8 +147,8 @@ void b3WeldJoint::InitializeConstraints(const b3SolverData* data) { b3Quat dq = b3Conjugate(m_referenceRotation) * b3Conjugate(qA) * qB; - m_J1 = -0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT; - m_J2 = 0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT; + m_J1 = -0.5f * b3Mat34_Weld_P * b3Mat44_Quat(b3Conjugate(qA)) * b3Mat44_Projection(qB) * b3Mat43_PT; + m_J2 = 0.5f * b3Mat34_Weld_P * b3Mat44_Quat(b3Conjugate(qA)) * b3Mat44_Projection(qB) * b3Mat43_PT; m_J1T = b3Transpose(m_J1); m_J2T = b3Transpose(m_J2); @@ -266,16 +266,16 @@ bool b3WeldJoint::SolvePositionConstraints(const b3SolverData* data) float32 angularError = 0.0f; - { + { b3Quat dq = b3Conjugate(m_referenceRotation) * b3Conjugate(qA) * qB; - b3Vec4 dq_v = q_to_v(dq); + b3Vec4 dq_v = b3Vec4_Quat(dq); - b3Vec3 C = P * dq_v; + b3Vec3 C = b3Mat34_P * dq_v; angularError += b3Length(C); - b3Mat33 J1 = -0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT; - b3Mat33 J2 = 0.5f * P_lock * iQ_mat(b3Conjugate(qA)) * iP_mat(qB) * PT; + b3Mat33 J1 = -0.5f * b3Mat34_Weld_P * b3Mat44_Quat(b3Conjugate(qA)) * b3Mat44_Projection(qB) * b3Mat43_PT; + b3Mat33 J2 = 0.5f * b3Mat34_Weld_P * b3Mat44_Quat(b3Conjugate(qA)) * b3Mat44_Projection(qB) * b3Mat43_PT; b3Mat33 J1T = b3Transpose(J1); b3Mat33 J2T = b3Transpose(J2); diff --git a/src/bounce/dynamics/shapes/capsule_shape.cpp b/src/bounce/dynamics/shapes/capsule_shape.cpp index 76af86d..06da598 100644 --- a/src/bounce/dynamics/shapes/capsule_shape.cpp +++ b/src/bounce/dynamics/shapes/capsule_shape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/shapes/hull_shape.cpp b/src/bounce/dynamics/shapes/hull_shape.cpp index 7428eba..81be848 100644 --- a/src/bounce/dynamics/shapes/hull_shape.cpp +++ b/src/bounce/dynamics/shapes/hull_shape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -17,8 +17,10 @@ */ #include -#include #include +#include +#include +#include b3HullShape::b3HullShape() { @@ -159,7 +161,7 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const { - aabb->Compute(m_hull->vertices, m_hull->vertexCount, xf); + aabb->Set(m_hull->vertices, m_hull->vertexCount, xf); aabb->Extend(m_radius); } @@ -184,10 +186,45 @@ bool b3HullShape::TestSphere(const b3Sphere& sphere, const b3Transform& xf) cons bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf) const { - // Perform computations in the local space of the first hull. - b3Vec3 support = b3MulT(xf, sphere.vertex); - float32 radius = m_radius + sphere.radius; + b3Transform xf1 = xf; + b3Transform xf2 = b3Transform_identity; + + b3ShapeGJKProxy proxy1(this, 0); + b3GJKProxy proxy2; + proxy2.vertexCount = 1; + proxy2.vertexBuffer[0] = sphere.vertex; + proxy2.vertices = proxy2.vertexBuffer; + proxy2.radius = sphere.radius; + + b3GJKOutput gjk = b3GJK(xf1, proxy1, xf2, proxy2); + + float32 r1 = proxy1.radius; + float32 r2 = proxy2.radius; + + float32 totalRadius = r1 + r2; + + if (gjk.distance > totalRadius) + { + return false; + } + + if (gjk.distance > 0.0f) + { + b3Vec3 c1 = gjk.point1; + b3Vec3 c2 = gjk.point2; + b3Vec3 n = (c2 - c1) / gjk.distance; + + output->point = c1; + output->normal = n; + output->separation = gjk.distance - totalRadius; + + return true; + } + + // Perform computations in the local space of the first hull. + b3Vec3 support = b3MulT(xf1, sphere.vertex); + u32 maxIndex = ~0; float32 maxSeparation = -B3_MAX_FLOAT; @@ -196,7 +233,7 @@ bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, b3Plane plane = m_hull->GetPlane(i); float32 separation = b3Distance(support, plane); - if (separation > radius) + if (separation > totalRadius) { return false; } @@ -208,17 +245,14 @@ bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, } } - if (maxIndex != ~0) - { - b3Plane plane = b3Mul(xf, m_hull->GetPlane(maxIndex)); - output->point = b3ClosestPointOnPlane(sphere.vertex, plane); - output->separation = maxSeparation - radius; - output->normal = plane.normal; - return true; - } + B3_ASSERT(maxIndex != ~0); + + b3Plane plane = b3Mul(xf1, m_hull->GetPlane(maxIndex)); - B3_ASSERT(false); - return false; + output->point = b3ClosestPointOnPlane(sphere.vertex, plane); + output->separation = maxSeparation - totalRadius; + output->normal = plane.normal; + return true; } bool b3HullShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const diff --git a/src/bounce/dynamics/shapes/mesh_shape.cpp b/src/bounce/dynamics/shapes/mesh_shape.cpp index 7d2fa9e..55724e6 100644 --- a/src/bounce/dynamics/shapes/mesh_shape.cpp +++ b/src/bounce/dynamics/shapes/mesh_shape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -56,7 +56,7 @@ void b3MeshShape::ComputeMass(b3MassData* massData, float32 density) const void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf) const { - output->Compute(m_mesh->vertices, m_mesh->vertexCount, xf); + output->Set(m_mesh->vertices, m_mesh->vertexCount, xf); output->Extend(m_radius); } @@ -88,10 +88,20 @@ bool b3MeshShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, return false; } +bool b3MeshShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, const b3Transform& xf, u32 index) const +{ + B3_NOT_USED(output); + B3_NOT_USED(sphere); + B3_NOT_USED(xf); + B3_NOT_USED(index); + return false; +} + bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf, u32 index) const { B3_ASSERT(index < m_mesh->triangleCount); b3Triangle* triangle = m_mesh->triangles + index; + b3Vec3 v1 = m_mesh->vertices[triangle->v1]; b3Vec3 v2 = m_mesh->vertices[triangle->v2]; b3Vec3 v3 = m_mesh->vertices[triangle->v3]; @@ -99,78 +109,24 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, // Put the ray into the mesh's frame of reference. b3Vec3 p1 = b3MulT(xf, input.p1); b3Vec3 p2 = b3MulT(xf, input.p2); - b3Vec3 d = p2 - p1; - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - n.Normalize(); - float32 numerator = b3Dot(n, v1 - p1); - float32 denominator = b3Dot(n, d); + b3RayCastInput subInput; + subInput.p1 = p1; + subInput.p2 = p2; + subInput.maxFraction = input.maxFraction; - if (denominator == 0.0f) + b3RayCastOutput subOutput; + if (b3RayCast(&subOutput, &subInput, v1, v2, v3)) { - return false; - } - - float32 t = numerator / denominator; - - // Is the intersection not on the segment? - if (t < 0.0f || input.maxFraction < t) - { - return false; - } - - b3Vec3 q = p1 + t * d; - - // Barycentric coordinates for q - b3Vec3 Q = q; - b3Vec3 A = v1; - b3Vec3 B = v2; - b3Vec3 C = v3; - - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - float32 u = b3Dot(QB_x_QC, AB_x_AC); - float32 v = b3Dot(QC_x_QA, AB_x_AC); - float32 w = b3Dot(QA_x_QB, AB_x_AC); - - // This tolerance helps intersections lying on - // shared edges to not be missed. - const float32 kTol = -0.005f; - - // Is the intersection on the triangle? - if (u > kTol && v > kTol && w > kTol) - { - output->fraction = t; - - // Does the ray start from below or above the triangle? - if (numerator > 0.0f) - { - output->normal = -b3Mul(xf.rotation, n); - } - else - { - output->normal = b3Mul(xf.rotation, n); - } - + output->fraction = subOutput.fraction; + output->normal = xf.rotation * subOutput.normal; return true; } return false; } -struct b3MeshRayCastCallback +struct b3MeshShapeRayCastCallback { float32 Report(const b3RayCastInput& subInput, u32 proxyId) { @@ -202,7 +158,7 @@ struct b3MeshRayCastCallback bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const { - b3MeshRayCastCallback callback; + b3MeshShapeRayCastCallback callback; callback.input = input; callback.mesh = this; callback.xf = xf; diff --git a/src/bounce/dynamics/shapes/shape.cpp b/src/bounce/dynamics/shapes/shape.cpp index 4fe4ef5..077b206 100644 --- a/src/bounce/dynamics/shapes/shape.cpp +++ b/src/bounce/dynamics/shapes/shape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -65,6 +65,11 @@ void b3Shape::DestroyContacts() } } +const b3AABB3& b3Shape::GetAABB() const +{ + return m_body->GetWorld()->m_contactMan.m_broadPhase.GetAABB(m_broadPhaseID); +} + void b3Shape::Dump(u32 bodyIndex) const { switch (m_type) diff --git a/src/bounce/dynamics/shapes/sphere_shape.cpp b/src/bounce/dynamics/shapes/sphere_shape.cpp index 8d3819c..ba600c2 100644 --- a/src/bounce/dynamics/shapes/sphere_shape.cpp +++ b/src/bounce/dynamics/shapes/sphere_shape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/world.cpp b/src/bounce/dynamics/world.cpp index aa9647a..700f284 100644 --- a/src/bounce/dynamics/world.cpp +++ b/src/bounce/dynamics/world.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -32,7 +31,6 @@ extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters; extern bool b3_convexCache; b3World::b3World() : - m_clothBlocks(sizeof(b3Cloth)), m_bodyBlocks(sizeof(b3Body)) { b3_allocCalls = 0; @@ -54,14 +52,6 @@ b3World::b3World() : b3World::~b3World() { - b3Cloth* c = m_clothList.m_head; - while (c) - { - b3Cloth* c0 = c; - c = c->m_next; - c0->~b3Cloth(); - } - b3Body* b = m_bodyList.m_head; while (b) { @@ -93,21 +83,6 @@ void b3World::SetSleeping(bool flag) } } -b3Cloth* b3World::CreateCloth(const b3ClothDef& def) -{ - void* mem = m_clothBlocks.Allocate(); - b3Cloth* c = new(mem) b3Cloth(def, this); - m_clothList.PushFront(c); - return c; -} - -void b3World::DestroyCloth(b3Cloth* c) -{ - m_clothList.Remove(c); - c->~b3Cloth(); - m_clothBlocks.Free(c); -} - b3Body* b3World::CreateBody(const b3BodyDef& def) { void* mem = m_bodyBlocks.Allocate(); @@ -166,11 +141,6 @@ void b3World::Step(float32 dt, u32 velocityIterations, u32 positionIterations) { Solve(dt, velocityIterations, positionIterations); } - - //SolveTOI - - // Step cloth dynamics - StepCloth(dt); } void b3World::Solve(float32 dt, u32 velocityIterations, u32 positionIterations) @@ -366,16 +336,6 @@ void b3World::Solve(float32 dt, u32 velocityIterations, u32 positionIterations) } } -void b3World::StepCloth(float32 dt) -{ - B3_PROFILE("Step Cloth"); - - for (b3Cloth* c = m_clothList.m_head; c; c = c->GetNext()) - { - c->Step(dt, m_gravity); - } -} - struct b3ShapeRayCastCallback { float32 Report(const b3RayCastInput& input, u32 proxyId) @@ -411,7 +371,7 @@ struct b3ShapeRayCastCallback const b3BroadPhase* broadPhase; }; -void b3World::RayCastShape(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const +void b3World::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const { b3RayCastInput input; input.p1 = p1; @@ -456,7 +416,7 @@ struct b3RayCastSingleShapeCallback const b3BroadPhase* broadPhase; }; -bool b3World::RayCastSingleShape(b3RayCastSingleShapeOutput* output, const b3Vec3& p1, const b3Vec3& p2) const +bool b3World::RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const { b3RayCastInput input; input.p1 = p1; @@ -489,52 +449,6 @@ bool b3World::RayCastSingleShape(b3RayCastSingleShapeOutput* output, const b3Vec return false; } -void b3World::RayCastCloth(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const -{ - for (b3Cloth* c = m_clothList.m_head; c; c = c->m_next) - { - c->RayCast(listener, p1, p2); - } -} - -bool b3World::RayCastSingleCloth(b3RayCastSingleClothOutput* output, const b3Vec3& p1, const b3Vec3& p2) const -{ - b3Cloth* cloth0 = NULL; - b3ClothRayCastSingleOutput output0; - output0.fraction = B3_MAX_FLOAT; - - for (b3Cloth* c = m_clothList.m_head; c; c = c->m_next) - { - b3ClothRayCastSingleOutput subOutput; - if (c->RayCastSingle(&subOutput, p1, p2)) - { - if (subOutput.fraction < output0.fraction) - { - cloth0 = c; - output0 = subOutput; - } - } - } - - if (cloth0 != NULL) - { - u32 triangle = output0.triangle; - float32 fraction = output0.fraction; - b3Vec3 point = (1.0f - fraction) * p1 + fraction * p2; - b3Vec3 normal = output0.normal; - - output->cloth = cloth0; - output->triangle = triangle; - output->point = point; - output->normal = normal; - output->fraction = fraction; - - return true; - } - - return false; -} - struct b3QueryAABBCallback { bool Report(u32 proxyID) diff --git a/src/bounce/meshgen/cylinder_mesh.cpp b/src/bounce/meshgen/cylinder_mesh.cpp new file mode 100644 index 0000000..9d8f99d --- /dev/null +++ b/src/bounce/meshgen/cylinder_mesh.cpp @@ -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. +*/ + +#include +#include + +void cymCreateMesh(cymMesh& output, u32 segments) +{ + B3_ASSERT(segments > 2); + + u32 vertexCount = 2 * segments; + b3Vec3* vertices = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3)); + b3Vec3* normals = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3)); + u32 indexCount = 3 * (2 * segments) + 2 * 3 * (segments - 2); + u32* indices = (u32*)b3Alloc(indexCount * sizeof(u32)); + + float32 angle = 2.0f * B3_PI / float32(segments); + b3Quat q(b3Vec3_y, angle); + + // Lower + b3Vec3 v(1.0f, -0.5f, 0.0f); + for (u32 i = 0; i < segments; ++i) + { + vertices[i] = v; + v = b3Mul(q, v); + } + + // Upper + v.Set(1.0f, 0.5f, 0.0f); + for (u32 i = 0; i < segments; ++i) + { + vertices[segments + i] = v; + v = b3Mul(q, v); + } + + u32 idx = 0; + + // Side triangles + for (u32 i = 0; i < segments; ++i) + { + u32 i1 = i; + u32 i2 = i1 + 1 < segments ? i1 + 1 : 0; + + u32 i3 = segments + i; + u32 i4 = i3 + 1 < 2 * segments ? i3 + 1 : segments; + + indices[idx++] = i1; + indices[idx++] = i2; + indices[idx++] = i4; + + indices[idx++] = i4; + indices[idx++] = i3; + indices[idx++] = i1; + } + + // Side normals + for (u32 i = 0; i < vertexCount; ++i) + { + normals[i].SetZero(); + } + + for (u32 i = 0; i < idx / 3; ++i) + { + u32 i1 = indices[3 * i + 0]; + u32 i2 = indices[3 * i + 1]; + u32 i3 = indices[3 * i + 2]; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); + n.Normalize(); + + normals[i1] += n; + normals[i2] += n; + normals[i3] += n; + } + + for (u32 i = 0; i < vertexCount; ++i) + { + normals[i].Normalize(); + } + + // Lower. Reverse loop to ensure CCW + u32 i1 = segments - 1; + for (u32 i2 = i1 - 1; i2 > 0; --i2) + { + u32 i3 = i2 - 1; + + indices[idx++] = i1; + indices[idx++] = i2; + indices[idx++] = i3; + } + + // Upper + i1 = segments; + for (u32 i2 = i1 + 1; i2 < 2 * segments - 1; ++i2) + { + u32 i3 = i2 + 1; + + indices[idx++] = i1; + indices[idx++] = i2; + indices[idx++] = i3; + } + + B3_ASSERT(idx == indexCount); + + output.vertexCount = vertexCount; + output.vertices = vertices; + output.normals = normals; + output.indexCount = indexCount; + output.indices = indices; +} \ No newline at end of file diff --git a/src/bounce/meshgen/sphere_mesh.cpp b/src/bounce/meshgen/sphere_mesh.cpp new file mode 100644 index 0000000..8b6f882 --- /dev/null +++ b/src/bounce/meshgen/sphere_mesh.cpp @@ -0,0 +1,258 @@ +/* +* 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 + +static inline void smAddVertex(smMesh& mesh, float32 x, float32 y, float32 z) +{ + mesh.vertices[mesh.vertexCount++].Set(x, y, z); +} + +static inline void smAddTriangle(smMesh& mesh, u32 v1, u32 v2, u32 v3) +{ + mesh.indices[mesh.indexCount++] = v1; + mesh.indices[mesh.indexCount++] = v2; + mesh.indices[mesh.indexCount++] = v3; +} + +static inline void smSetAsOctahedron(smMesh& mesh) +{ + B3_ASSERT(mesh.vertexCount == 0); + + smAddVertex(mesh, 0.0f, -1.0f, 0.0f); + smAddVertex(mesh, 0.0f, 0.0f, 1.0f); + smAddVertex(mesh, -1.0f, 0.0f, 0.0f); + smAddVertex(mesh, 0.0f, 0.0f, -1.0f); + smAddVertex(mesh, 1.0f, 0.0f, 0.0f); + smAddVertex(mesh, 0.0f, 1.0f, 0.0f); + + B3_ASSERT(mesh.indexCount == 0); + + smAddTriangle(mesh, 0, 1, 2); + smAddTriangle(mesh, 0, 2, 3); + smAddTriangle(mesh, 0, 3, 4); + smAddTriangle(mesh, 0, 4, 1); + + smAddTriangle(mesh, 5, 2, 1); + smAddTriangle(mesh, 5, 3, 2); + smAddTriangle(mesh, 5, 4, 3); + smAddTriangle(mesh, 5, 1, 4); +} + +struct smEdge +{ + u32 v1, v2; +}; + +struct smEdgeVertexPair +{ + smEdge edge; + u32 vertex; +}; + +struct smEdgeVertexMap +{ + u32 pairCount; + smEdgeVertexPair* pairs; +}; + +static inline void smAddPair(smEdgeVertexMap& map, const smEdgeVertexPair& pair) +{ + map.pairs[map.pairCount++] = pair; +} + +static inline smEdgeVertexPair* smFind(smEdgeVertexMap& map, u32 v1, u32 v2) +{ + for (u32 i = 0; i < map.pairCount; ++i) + { + smEdgeVertexPair* pair = map.pairs + i; + + if (pair->edge.v1 == v1 && pair->edge.v2 == v2) + { + return pair; + } + } + return nullptr; +} + +static inline u32 smSubdivideEdge(smMesh& out, smEdgeVertexMap& map, + const smMesh& in, + u32 i1, u32 i2) +{ + smEdgeVertexPair* pair = smFind(map, i2, i1); + + if (pair) + { + return pair->vertex; + } + + smEdge newEdge; + newEdge.v1 = i1; + newEdge.v2 = i2; + + u32 newVertex = out.vertexCount; + + b3Vec3 v1 = in.vertices[i1]; + b3Vec3 v2 = in.vertices[i2]; + b3Vec3 v = 0.5f * (v1 + v2); + v.Normalize(); + + smAddVertex(out, v.x, v.y, v.z); + + smEdgeVertexPair newPair; + newPair.edge = newEdge; + newPair.vertex = newVertex; + + smAddPair(map, newPair); + + return newVertex; +} + +static void smSubdivideMesh(smMesh& out, const smMesh& in, smEdgeVertexMap& map) +{ + B3_ASSERT(out.vertexCount == 0); + B3_ASSERT(out.indexCount == 0); + B3_ASSERT(map.pairCount == 0); + + out.vertexCount = in.vertexCount; + memcpy(out.vertices, in.vertices, in.vertexCount * sizeof(b3Vec3)); + + for (u32 i = 0; i < in.indexCount / 3; ++i) + { + u32 vi1 = in.indices[3 * i + 0]; + u32 vi2 = in.indices[3 * i + 1]; + u32 vi3 = in.indices[3 * i + 2]; + + u32 vi4 = smSubdivideEdge(out, map, in, vi1, vi2); + u32 vi5 = smSubdivideEdge(out, map, in, vi2, vi3); + u32 vi6 = smSubdivideEdge(out, map, in, vi3, vi1); + + smAddTriangle(out, vi1, vi4, vi6); + smAddTriangle(out, vi4, vi2, vi5); + smAddTriangle(out, vi5, vi3, vi6); + smAddTriangle(out, vi4, vi5, vi6); + } +} + +static inline void smCount(u32& inVertexCapacity, u32& inIndexCount, + u32& outVertexCapacity, u32& outIndexCount, + u32& edgeVertexPairCapacity, + u32 subdivisions) +{ + inVertexCapacity = 6; + u32 inTriangleCount = 8; + + outVertexCapacity = 0; + u32 outTriangleCount = 0; + + edgeVertexPairCapacity = 0; + + for (u32 i = 0; i < subdivisions; ++i) + { + outVertexCapacity = inVertexCapacity + 3 * inTriangleCount; + outTriangleCount = 4 * inTriangleCount; + + edgeVertexPairCapacity = 3 * inTriangleCount; + + inVertexCapacity = outVertexCapacity; + inTriangleCount = outTriangleCount; + } + + inIndexCount = 3 * inTriangleCount; + outIndexCount = 3 * outTriangleCount; +} + +void smCreateMesh(smMesh& output, u32 subdivisions) +{ + B3_ASSERT(output.vertexCount == 0); + B3_ASSERT(output.indexCount == 0); + + u32 inVertexCapacity, inIndexCount; + u32 outVertexCapacity, outIndexCount; + u32 edgeVertexPairCapacity; + smCount(inVertexCapacity, inIndexCount, outVertexCapacity, outIndexCount, edgeVertexPairCapacity, subdivisions); + + u32 byteCount = 0; + byteCount += inVertexCapacity * sizeof(b3Vec3); + byteCount += inIndexCount * sizeof(u32); + byteCount += outVertexCapacity * sizeof(b3Vec3); + byteCount += outIndexCount * sizeof(u32); + byteCount += edgeVertexPairCapacity * sizeof(smEdgeVertexPair); + + u8* bytes = (u8*)b3Alloc(byteCount); + + b3Vec3* inVertex = (b3Vec3*)bytes; + u32* inIndex = (u32*)((u8*)(inVertex) + inVertexCapacity * sizeof(b3Vec3)); + b3Vec3* outVertex = (b3Vec3*) ((u8*)(inIndex) + inIndexCount * sizeof(u32)); + u32* outIndex = (u32*)((u8*)(outVertex) + outVertexCapacity * sizeof(b3Vec3)); + smEdgeVertexPair* pairs = (smEdgeVertexPair*)((u8*)(outIndex) + outIndexCount * sizeof(u32)); + + smMesh in; + in.vertexCount = 0; + in.vertices = inVertex; + in.normals = nullptr; + in.indexCount = 0; + in.indices = inIndex; + + smSetAsOctahedron(in); + + for (u32 i = 0; i < subdivisions; ++i) + { + smMesh out; + out.vertexCount = 0; + out.vertices = outVertex; + out.indexCount = 0; + out.indices = outIndex; + + smEdgeVertexMap map; + map.pairCount = 0; + map.pairs = pairs; + + smSubdivideMesh(out, in, map); + + // in = out + in.vertexCount = out.vertexCount; + in.vertices = inVertex; + memcpy(in.vertices, out.vertices, out.vertexCount * sizeof(b3Vec3)); + + in.indexCount = out.indexCount; + in.indices = inIndex; + memcpy(in.indices, out.indices, out.indexCount * sizeof(u32)); + + out.vertices = nullptr; + out.normals = nullptr; + out.indices = nullptr; + } + + output.vertexCount = in.vertexCount; + output.vertices = (b3Vec3*)b3Alloc(in.vertexCount * sizeof(b3Vec3)); + memcpy(output.vertices, in.vertices, in.vertexCount * sizeof(b3Vec3)); + + output.normals = (b3Vec3*)b3Alloc(in.vertexCount * sizeof(b3Vec3)); + memcpy(output.normals, output.vertices, output.vertexCount * sizeof(b3Vec3)); + + output.indexCount = in.indexCount; + output.indices = (u32*)b3Alloc(in.indexCount * sizeof(u32)); + memcpy(output.indices, in.indices, in.indexCount * sizeof(u32)); + + b3Free(bytes); + + in.vertices = nullptr; + in.normals = nullptr; + in.indices = nullptr; +} \ No newline at end of file diff --git a/src/bounce/quickhull/qh_hull.cpp b/src/bounce/quickhull/qh_hull.cpp index b2fb740..2b11141 100644 --- a/src/bounce/quickhull/qh_hull.cpp +++ b/src/bounce/quickhull/qh_hull.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 diff --git a/src/bounce/dynamics/rope/rope.cpp b/src/bounce/rope/rope.cpp similarity index 97% rename from src/bounce/dynamics/rope/rope.cpp rename to src/bounce/rope/rope.cpp index 6ddec52..d74582d 100644 --- a/src/bounce/dynamics/rope/rope.cpp +++ b/src/bounce/rope/rope.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,8 +16,8 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include +#include +#include #include struct b3RopeBody @@ -480,7 +480,7 @@ void b3Rope::Draw() const b3RopeBody* b = m_links; b3Draw_draw->DrawTransform(b->m_X); - b3Draw_draw->DrawSolidSphere(b->m_X.position, 0.2f, b3Color_green); + b3Draw_draw->DrawSolidSphere(b->m_X.position, 0.2f, b->m_X.rotation, b3Color_green); } for (u32 i = 1; i < m_count; ++i) @@ -498,6 +498,6 @@ void b3Rope::Draw() const b3Draw_draw->DrawPoint(X_J0.position, 5.0f, b3Color_red); b3Draw_draw->DrawTransform(b->m_X); - b3Draw_draw->DrawSolidSphere(b->m_X.position, 0.2f, b3Color_green); + b3Draw_draw->DrawSolidSphere(b->m_X.position, 0.2f, b->m_X.rotation, b3Color_green); } } \ No newline at end of file diff --git a/src/bounce/softbody/contacts/softbody_contact_solver.cpp b/src/bounce/softbody/contacts/softbody_contact_solver.cpp new file mode 100644 index 0000000..21721c9 --- /dev/null +++ b/src/bounce/softbody/contacts/softbody_contact_solver.cpp @@ -0,0 +1,413 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include + +b3SoftBodyContactSolver::b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def) +{ + m_allocator = def.allocator; + + m_positions = def.positions; + m_velocities = def.velocities; + + m_bodyContactCount = def.bodyContactCount; + m_bodyContacts = def.bodyContacts; +} + +b3SoftBodyContactSolver::~b3SoftBodyContactSolver() +{ + m_allocator->Free(m_bodyPositionConstraints); + m_allocator->Free(m_bodyVelocityConstraints); +} + +void b3SoftBodyContactSolver::InitializeBodyContactConstraints() +{ + m_bodyVelocityConstraints = (b3SoftBodySolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactVelocityConstraint)); + m_bodyPositionConstraints = (b3SoftBodySolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactPositionConstraint)); + + 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->m_n1->m_vertex; + vc->bodyB = c->m_s2->GetBody(); + + vc->invMassA = c->m_n1->m_type == e_staticSoftBodyNode ? 0.0f : c->m_n1->m_invMass; + vc->invMassB = vc->bodyB->GetInverseMass(); + + vc->invIA.SetZero(); + vc->invIB = vc->bodyB->GetWorldInverseInertia(); + + vc->friction = b3MixFriction(c->m_n1->m_friction, c->m_s2->GetFriction()); + + pc->indexA = c->m_n1->m_vertex; + pc->bodyB = vc->bodyB; + + pc->invMassA = c->m_n1->m_type == e_staticSoftBodyNode ? 0.0f : c->m_n1->m_invMass; + pc->invMassB = vc->bodyB->m_invMass; + + pc->invIA.SetZero(); + pc->invIB = vc->bodyB->m_worldInvI; + + pc->radiusA = c->m_n1->m_radius; + pc->radiusB = c->m_s2->m_radius; + + pc->localCenterA.SetZero(); + pc->localCenterB = pc->bodyB->m_sweep.localCenter; + + pc->normalA = c->m_normal1; + pc->localPointA = c->m_localPoint1; + pc->localPointB = c->m_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 = m_positions[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->m_tangent1; + vc->tangent2 = c->m_tangent2; + 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->m_normalImpulse; + vc->tangentImpulse = c->m_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() +{ + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + + u32 indexA = vc->indexA; + b3Body* bodyB = vc->bodyB; + + b3Vec3 vA = m_velocities[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); + + m_velocities[indexA] = vA; + + bodyB->SetLinearVelocity(vB); + bodyB->SetAngularVelocity(wB); + } +} + +void b3SoftBodyContactSolver::SolveBodyContactVelocityConstraints() +{ + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + + u32 indexA = vc->indexA; + b3Body* bodyB = vc->bodyB; + + b3Vec3 vA = m_velocities[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); + } + + m_velocities[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->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse = vc->tangentImpulse; + } +} + +struct b3SoftBodySolverBodyContactSolverPoint +{ + void Initialize(const b3SoftBodySolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) + { + b3Vec3 nA = pc->normalA; + + b3Vec3 cA = xfA * pc->localPointA; + b3Vec3 cB = xfB * pc->localPointB; + + float32 rA = pc->radiusA; + float32 rB = pc->radiusB; + + b3Vec3 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() +{ + 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 = m_positions[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); + + b3SoftBodySolverBodyContactSolverPoint 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(); + + m_positions[indexA] = cA; + + bodyB->m_sweep.worldCenter = cB; + bodyB->m_sweep.orientation = qB; + } + + return minSeparation >= -3.0f * B3_LINEAR_SLOP; +} \ No newline at end of file diff --git a/src/bounce/softbody/contacts/softbody_node_body_contact.cpp b/src/bounce/softbody/contacts/softbody_node_body_contact.cpp new file mode 100644 index 0000000..483ae3a --- /dev/null +++ b/src/bounce/softbody/contacts/softbody_node_body_contact.cpp @@ -0,0 +1,62 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +void b3NodeBodyContact::Update() +{ + b3Sphere sphere; + sphere.radius = m_n1->m_radius; + sphere.vertex = m_n1->m_position; + + b3Shape* shape = m_s2; + b3Body* body = shape->GetBody(); + b3Transform xf = body->GetTransform(); + + b3TestSphereOutput out; + if (shape->TestSphere(&out, sphere, xf) == false) + { + m_active = false; + return; + } + + m_active = true; + m_normal1 = -out.normal; + m_localPoint1.SetZero(); + m_localPoint2 = body->GetLocalPoint(out.point); + m_tangent1 = b3Perp(m_normal1); + m_tangent2 = b3Cross(m_tangent1, m_normal1); +} + +void b3NodeBodyContactWorldPoint::Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) +{ + b3Vec3 nA = c->m_normal1; + + b3Vec3 cA = xfA * c->m_localPoint1; + b3Vec3 cB = xfB * c->m_localPoint2; + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = b3Dot(cB - cA, nA) - rA - rB; +} diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp new file mode 100644 index 0000000..1571dc3 --- /dev/null +++ b/src/bounce/softbody/softbody.cpp @@ -0,0 +1,874 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include + +// C = A * B +static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN) +{ + B3_ASSERT(AN == BM); + + 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]; + } + } + } +} + +// B = A^T +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]; + } + } +} + +// Compute the elasticity matrix given Young modulus and Poisson's ratio +// This is a 6 x 6 matrix +static B3_FORCE_INLINE void b3ComputeD(float32 out[36], + float32 E, float32 nu) +{ + float32 lambda = (nu * E) / ((1 + nu) * (1 - 2 * nu)); + float32 mu = E / (2 * (1 + nu)); + + float32 D[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] = D[i]; + } +} + +// Compute B = S * N, +// where S is the operational matrix and N are the shape functions +// This is a 6 x 12 matrix +// A derivation and corresponding simplification for this matrix +// can be found here: +// https://github.com/erleben/OpenTissue/blob/master/OpenTissue/dynamics/fem/fem_compute_b.h +static B3_FORCE_INLINE void b3ComputeB(float32 out[72], + const b3Mat33& invE) +{ + // cofactor = det(E)^-1 * cofactor(E) + b3Mat33 cofactor = b3Transpose(invE); + + // minor = det(E)^-1 * minor(E) + b3Mat33 minor; + + minor.x.x = cofactor.x.x; + minor.x.y = -cofactor.x.y; + minor.x.z = cofactor.x.z; + + minor.y.x = -cofactor.y.x; + minor.y.y = cofactor.y.y; + minor.y.z = -cofactor.y.z; + + minor.z.x = cofactor.z.x; + minor.z.y = -cofactor.z.y; + minor.z.z = cofactor.z.z; + + float32 e11 = -minor.x.x; // -det(E)^-1 * det(E_11) + float32 e12 = minor.y.x; // det(E)^-1 * det(E_12) + float32 e13 = -minor.z.x; // -det(E)^-1 * det(E_13) + + float32 e21 = minor.x.y; // det(E)^-1 * det(E_21) + float32 e22 = -minor.y.y; // -det(E)^-1 * det(E_22) + float32 e23 = minor.z.y; // det(E)^-1 * det(E_23) + + float32 e31 = -minor.x.z; // -det(E)^-1 * det(E_31) + float32 e32 = minor.y.z; // det(E)^-1 * det(E_32) + float32 e33 = -minor.z.z; // -det(E)^-1 * det(E_33) + + float32 b1 = -e11 - e12 - e13; + float32 c1 = -e21 - e22 - e23; + float32 d1 = -e31 - e32 - e33; + + float32 b2 = e11; + float32 c2 = e21; + float32 d2 = e31; + + float32 b3 = e12; + float32 c3 = e22; + float32 d3 = e32; + + float32 b4 = e13; + float32 c4 = e23; + float32 d4 = e33; + + float32 B[72] = + { + b1, 0, 0, c1, d1, 0, + 0, c1, 0, b1, 0, d1, + 0, 0, d1, 0, b1, c1, + + b2, 0, 0, c2, d2, 0, + 0, c2, 0, b2, 0, d2, + 0, 0, d2, 0, b2, c2, + + b3, 0, 0, c3, d3, 0, + 0, c3, 0, b3, 0, d3, + 0, 0, d3, 0, b3, c3, + + b4, 0, 0, c4, d4, 0, + 0, c4, 0, b4, 0, d4, + 0, 0, d4, 0, b4, c4, + }; + + 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]; + k21 = b3Transpose(k12); + + // 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]; + k31 = b3Transpose(k13); + + // 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]; + k32 = b3Transpose(k23); + + // 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]; + k41 = b3Transpose(k14); + + // 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]; + k42 = b3Transpose(k24); + + // 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]; + k43 = b3Transpose(k34); + + // 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_gravity.SetZero(); + m_world = nullptr; + m_contactManager.m_body = this; + + const b3SoftBodyMesh* m = m_mesh; + + // Initialize nodes + m_nodes = (b3SoftBodyNode*)b3Alloc(m->vertexCount * sizeof(b3SoftBodyNode)); + 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_massDamping = 0.0f; + n->m_radius = 0.0f; + n->m_friction = 0.0f; + n->m_userData = nullptr; + n->m_vertex = i; + + b3AABB3 aabb; + aabb.Set(n->m_position, 0.0f); + + n->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, n); + } + + // 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; + + e->E = def.E; + e->nu = def.nu; + e->c_yield = def.c_yield; + e->c_creep = def.c_creep; + e->c_max = def.c_max; + + 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 V = b3Volume(p1, p2, p3, p4); + + B3_ASSERT(V > 0.0f); + + b3Vec3 e1 = p2 - p1; + b3Vec3 e2 = p3 - p1; + b3Vec3 e3 = p4 - p1; + + b3Mat33 E(e1, e2, e3); + + e->invE = b3Inverse(E); + + // 6 x 6 + float32 D[36]; + b3ComputeD(D, e->E, e->nu); + + // 6 x 12 + float32* B = e->B; + b3ComputeB(B, e->invE); + + // 12 x 6 + float32 BT[72]; + b3Transpose(BT, B, 6, 12); + + // 12 x 6 + float32 BT_D[72]; + b3Mul(BT_D, BT, 12, 6, D, 6, 6); + + // 12 x 12 + float32 BT_D_B[144]; + b3Mul(BT_D_B, BT_D, 12, 6, B, 6, 12); + for (u32 i = 0; i < 144; ++i) + { + BT_D_B[i] *= V; + } + + b3SetK(e->K, BT_D_B); + + // 12 x 6 + float32* P = e->P; + b3Mul(P, BT, 12, 6, D, 6, 6); + for (u32 i = 0; i < 72; ++i) + { + P[i] *= V; + } + + for (u32 i = 0; i < 6; ++i) + { + e->epsilon_plastic[i] = 0.0f; + } + } + + // 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; +} + +b3SoftBodyElement* b3SoftBody::GetTetrahedronElement(u32 i) +{ + B3_ASSERT(i < m_mesh->tetrahedronCount); + return m_elements + 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::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) +{ + B3_PROFILE("Soft Body Solve"); + + b3SoftBodySolverDef def; + def.body = this; + + b3SoftBodySolver solver(def); + + // Push the body contacts + for (b3NodeBodyContact* c = m_contactManager.m_nodeBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_active) + { + solver.Add(c); + } + } + + solver.Solve(dt, gravity, velocityIterations, positionIterations); +} + +void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations) +{ + B3_PROFILE("Soft Body Step"); + + // Update contacts + m_contactManager.UpdateBodyContacts(); + + // 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(); + } + + // Synchronize nodes + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + b3SoftBodyNode* n = m_nodes + i; + + if (n->m_type == e_staticSoftBodyNode) + { + continue; + } + + b3Vec3 displacement = dt * n->m_velocity; + + n->Synchronize(displacement); + } + + // Find new contacts + m_contactManager.FindNewBodyContacts(); +} + +void b3SoftBody::SetWorld(b3World* world) +{ + if (!world && m_world) + { + // Destroy body contacts + b3NodeBodyContact* c = m_contactManager.m_nodeBodyContactList.m_head; + while (c) + { + b3NodeBodyContact* kaboom = c; + c = c->m_next; + m_contactManager.Destroy(kaboom); + } + } + + m_world = world; +} + +void b3SoftBody::Draw() const +{ + 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_kinematicSoftBodyNode) + { + b3Draw_draw->DrawPoint(v, 4.0f, b3Color_blue); + } + + 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); + } +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_contact_manager.cpp b/src/bounce/softbody/softbody_contact_manager.cpp new file mode 100644 index 0000000..68bbab4 --- /dev/null +++ b/src/bounce/softbody/softbody_contact_manager.cpp @@ -0,0 +1,165 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3SoftBodyContactManager::b3SoftBodyContactManager() : + m_nodeBodyContactBlocks(sizeof(b3NodeBodyContact)) +{ + +} + +class b3SoftBodyContactManagerFindNewBodyContactsQueryListener : public b3QueryListener +{ +public: + virtual bool ReportShape(b3Shape* s2) + { + cm->AddNSPair(n1, s2); + + // Keep looking for overlaps + return true; + } + + b3SoftBodyContactManager* cm; + b3SoftBodyNode* n1; +}; + +void b3SoftBodyContactManager::FindNewBodyContacts() +{ + B3_PROFILE("Soft Body Find New Body Contacts"); + + // Is there a world attached to this body? + if (m_body->m_world == nullptr) + { + return; + } + + for (u32 i = 0; i < m_body->m_mesh->vertexCount; ++i) + { + b3SoftBodyNode* n = m_body->m_nodes + i; + + if (n->m_type != e_dynamicSoftBodyNode) + { + continue; + } + + b3AABB3 aabb = m_broadPhase.GetAABB(n->m_broadPhaseId); + + b3SoftBodyContactManagerFindNewBodyContactsQueryListener listener; + listener.cm = this; + listener.n1 = n; + + m_body->m_world->QueryAABB(&listener, aabb); + } +} + +void b3SoftBodyContactManager::AddNSPair(b3SoftBodyNode* n1, b3Shape* s2) +{ + // Check if there is a contact between the two entities. + for (b3NodeBodyContact* c = m_nodeBodyContactList.m_head; c; c = c->m_next) + { + if (c->m_n1 == n1 && c->m_s2 == s2) + { + // A contact already exists. + return; + } + } + + bool isntDynamic1 = n1->m_type != e_dynamicSoftBodyNode; + bool isntDynamic2 = s2->GetBody()->GetType() != e_dynamicBody; + + if (isntDynamic1 && isntDynamic2) + { + // The entities must not collide with each other. + return; + } + + // Create a new contact. + b3NodeBodyContact* c = CreateNodeBodyContact(); + + c->m_n1 = n1; + c->m_s2 = s2; + c->m_active = false; + c->m_normalImpulse = 0.0f; + c->m_tangentImpulse.SetZero(); + + // Add the contact to the body contact list. + m_nodeBodyContactList.PushFront(c); +} + +void b3SoftBodyContactManager::UpdateBodyContacts() +{ + B3_PROFILE("Soft Body Update Body Contacts"); + + // Update the state of node-body contacts. + b3NodeBodyContact* c = m_nodeBodyContactList.m_head; + while (c) + { + bool isntDynamic1 = c->m_n1->m_type != e_dynamicSoftBodyNode; + bool isntDynamic2 = c->m_s2->GetBody()->GetType() != e_dynamicBody; + + // Cease the contact if entities must not collide with each other. + if (isntDynamic1 && isntDynamic2) + { + b3NodeBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + b3AABB3 aabb1 = m_broadPhase.GetAABB(c->m_n1->m_broadPhaseId); + b3AABB3 aabb2 = c->m_s2->GetAABB(); + + // Destroy the contact if entities AABBs are not overlapping. + bool overlap = b3TestOverlap(aabb1, aabb2); + if (overlap == false) + { + b3NodeBodyContact* quack = c; + c = c->m_next; + Destroy(quack); + continue; + } + + // The contact persists. + c->Update(); + + c = c->m_next; + } +} + +b3NodeBodyContact* b3SoftBodyContactManager::CreateNodeBodyContact() +{ + void* block = m_nodeBodyContactBlocks.Allocate(); + return new(block) b3NodeBodyContact(); +} + +void b3SoftBodyContactManager::Destroy(b3NodeBodyContact* c) +{ + m_nodeBodyContactList.Remove(c); + + c->~b3NodeBodyContact(); + + m_nodeBodyContactBlocks.Free(c); +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_force_solver.cpp b/src/bounce/softbody/softbody_force_solver.cpp new file mode 100644 index 0000000..a9eade5 --- /dev/null +++ b/src/bounce/softbody/softbody_force_solver.cpp @@ -0,0 +1,396 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include + +// 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; + +b3SoftBodyForceSolver::b3SoftBodyForceSolver(const b3SoftBodyForceSolverDef& def) +{ + m_body = def.body; + m_allocator = &m_body->m_stackAllocator; + m_mesh = m_body->m_mesh; + m_nodes = m_body->m_nodes; + m_elements = m_body->m_elements; +} + +b3SoftBodyForceSolver::~b3SoftBodyForceSolver() +{ + +} + +// Extract rotation from deformation +// https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf +static 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); +} + +// Solve A * x = b +static void b3SolveMPCG(b3DenseVec3& x, + const b3SparseMat33View& A, const b3DenseVec3& b, + const b3DenseVec3& z, const b3DiagMat33& S, u32 maxIterations = 20) +{ + B3_PROFILE("Soft Body Solve MPCG"); + + // Jacobi preconditioner + // P = diag(A) + b3DiagMat33 P(A.rowCount); + b3DiagMat33 invP(A.rowCount); + for (u32 i = 0; i < A.rowCount; ++i) + { + b3Mat33 a = A(i, i); + + // Sylvester Criterion to ensure PD-ness + B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f); + + B3_ASSERT(a.x.x > 0.0f); + float32 xx = 1.0f / a.x.x; + + B3_ASSERT(a.y.y > 0.0f); + float32 yy = 1.0f / a.y.y; + + B3_ASSERT(a.z.z > 0.0f); + float32 zz = 1.0f / a.z.z; + + P[i] = b3Diagonal(a.x.x, a.y.y, a.z.z); + invP[i] = b3Diagonal(xx, yy, zz); + } + + x = z; + + 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; +} + +// C = A * B +static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN) +{ + B3_ASSERT(AN == BM); + + 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]; + } + } + } +} + +// ||v|| +static B3_FORCE_INLINE float32 b3Length(float32* v, u32 n) +{ + float32 result = 0.0f; + for (u32 i = 0; i < n; ++i) + { + result += v[i] * v[i]; + } + return b3Sqrt(result); +} + +void b3SoftBodyForceSolver::Solve(float32 dt, const b3Vec3& gravity) +{ + float32 h = dt; + float32 inv_h = 1.0f / h; + + b3SparseMat33 M(m_mesh->vertexCount); + b3SparseMat33 C(m_mesh->vertexCount); + b3SparseMat33 K(m_mesh->vertexCount); + b3DenseVec3 x(m_mesh->vertexCount); + b3DenseVec3 p(m_mesh->vertexCount); + b3DenseVec3 v(m_mesh->vertexCount); + b3DenseVec3 fe(m_mesh->vertexCount); + b3DenseVec3 f0(m_mesh->vertexCount); + b3DenseVec3 f_plastic(m_mesh->vertexCount); + b3DenseVec3 z(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); + + // Rayleigh damping + // C = alpha * M + beta * K + // Here the stiffness coefficient beta is zero + C(i, i) = b3Diagonal(n->m_massDamping * n->m_mass); + + x[i] = m_mesh->vertices[i]; + p[i] = n->m_position; + v[i] = n->m_velocity; + fe[i] = n->m_force; + z[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 + f0.SetZero(); + f_plastic.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; + + float32* Be = e->B; + float32* Pe = e->P; + float32* epsilon_plastic = e->epsilon_plastic; + + 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]; + + b3Mat33 R; + if (b3_enableStiffnessWarping) + { + b3Vec3 e1 = p2 - p1; + b3Vec3 e2 = p3 - p1; + b3Vec3 e3 = p4 - p1; + + b3Mat33 E(e1, e2, e3); + + b3Mat33 A = E * e->invE; + + b3ExtractRotation(R, e->q, A); + } + else + { + R.SetIdentity(); + } + + 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; + } + } + + // Elasticity + 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]; + + // Plasticity + b3Vec3 ps[4] = { p1, p2, p3, p4 }; + + b3Vec3 RT_x_x0[4]; + for (u32 i = 0; i < 4; ++i) + { + RT_x_x0[i] = RT * ps[i] - xs[i]; + } + + // 6 x 1 + float32 epsilon_total[6]; + b3Mul(epsilon_total, Be, 6, 12, &RT_x_x0[0].x, 12, 1); + + // 6 x 1 + float32 epsilon_elastic[6]; + for (u32 i = 0; i < 6; ++i) + { + epsilon_elastic[i] = epsilon_total[i] - epsilon_plastic[i]; + } + + float32 len_epsilon_elastic = b3Length(epsilon_elastic, 6); + if (len_epsilon_elastic > e->c_yield) + { + float32 amount = h * b3Min(e->c_creep, inv_h); + for (u32 i = 0; i < 6; ++i) + { + epsilon_plastic[i] += amount * epsilon_elastic[i]; + } + } + + float32 len_epsilon_plastic = b3Length(epsilon_plastic, 6); + if (len_epsilon_plastic > e->c_max) + { + float32 scale = e->c_max / len_epsilon_plastic; + for (u32 i = 0; i < 6; ++i) + { + epsilon_plastic[i] *= scale; + } + } + + b3Vec3 fs_plastic[4]; + b3Mul(&fs_plastic[0].x, Pe, 12, 6, epsilon_plastic, 6, 1); + for (u32 i = 0; i < 4; ++i) + { + fs_plastic[i] = R * fs_plastic[i]; + } + + f_plastic[v1] += fs_plastic[0]; + f_plastic[v2] += fs_plastic[1]; + f_plastic[v3] += fs_plastic[2]; + f_plastic[v4] += fs_plastic[3]; + } + + f0 = -f0; + + b3SparseMat33 A = M + h * C + h * h * K; + + b3SparseMat33View viewA(A); + + b3DenseVec3 b = M * v - h * (K * p + f0 - (f_plastic + fe)); + + b3DenseVec3 sx(m_mesh->vertexCount); + b3SolveMPCG(sx, viewA, b, z, S); + + // Copy velocity back to the particle + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + m_nodes[i].m_velocity = sx[i]; + } +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_mesh.cpp b/src/bounce/softbody/softbody_mesh.cpp new file mode 100644 index 0000000..995e12a --- /dev/null +++ b/src/bounce/softbody/softbody_mesh.cpp @@ -0,0 +1,113 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + +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); + + B3_ASSERT(vertexCount == 0); + 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]; + } + + B3_ASSERT(tetrahedronCount == 0); + 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; + } +} + +void b3QSoftBodyMesh::SetAsCylinder(float32 radius, float32 ey, u32 segments) +{ + cymMesh mesh; + cymCreateMesh(mesh, segments); + + B3_ASSERT(vertexCount == 0); + 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]; + } + + B3_ASSERT(tetrahedronCount == 0); + 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; + } + + float32 height = 2.0f * ey; + + for (u32 i = 0; i < vertexCount; ++i) + { + vertices[i].x *= radius; + vertices[i].y *= height; + vertices[i].z *= radius; + } +} \ No newline at end of file diff --git a/examples/testbed/tests/cloth_test.h b/src/bounce/softbody/softbody_node.cpp similarity index 56% rename from examples/testbed/tests/cloth_test.h rename to src/bounce/softbody/softbody_node.cpp index 3f95cce..cd7bb55 100644 --- a/examples/testbed/tests/cloth_test.h +++ b/src/bounce/softbody/softbody_node.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* 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 @@ -16,37 +16,31 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CLOTH_TESH_H -#define CLOTH_TESH_H +#include +#include -class ClothTest : public Test +void b3SoftBodyNode::Synchronize(const b3Vec3& displacement) { -public: - ClothTest() + b3AABB3 aabb; + aabb.Set(m_position, m_radius); + + m_body->m_contactManager.m_broadPhase.MoveProxy(m_broadPhaseId, aabb, displacement); +} + +void b3SoftBodyNode::DestroyContacts() +{ + // Destroy body contacts + b3NodeBodyContact* c = m_body->m_contactManager.m_nodeBodyContactList.m_head; + while (c) { - m_world.SetGravity(b3Vec3(0.0f, -10.0f, 0.0f)); - m_cloth = nullptr; - } - - void Step() - { - Test::Step(); - - m_cloth->Draw(); - - if (m_clothDragger.IsDragging() == true) + if (c->m_n1 == this) { - g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white); + b3NodeBodyContact* quack = c; + c = c->m_next; + m_body->m_contactManager.Destroy(quack); + continue; } - extern u32 b3_clothSolverIterations; - g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations); - - float32 E = m_cloth->GetEnergy(); - g_draw->DrawString(b3Color_white, "E = %f", E); + c = c->m_next; } - - b3Cloth* m_cloth; -}; - -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp new file mode 100644 index 0000000..42b4ad6 --- /dev/null +++ b/src/bounce/softbody/softbody_solver.cpp @@ -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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +b3SoftBodySolver::b3SoftBodySolver(const b3SoftBodySolverDef& def) +{ + m_body = def.body; + m_allocator = &m_body->m_stackAllocator; + m_mesh = m_body->m_mesh; + m_nodes = m_body->m_nodes; + m_elements = m_body->m_elements; + m_bodyContactCapacity = m_body->m_contactManager.m_nodeBodyContactList.m_count; + m_bodyContactCount = 0; + m_bodyContacts = (b3NodeBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3NodeBodyContact*)); +} + +b3SoftBodySolver::~b3SoftBodySolver() +{ + m_allocator->Free(m_bodyContacts); +} + +void b3SoftBodySolver::Add(b3NodeBodyContact* c) +{ + m_bodyContacts[m_bodyContactCount++] = c; +} + +void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) +{ + { + // Solve internal dynamics + b3SoftBodyForceSolverDef forceSolverDef; + forceSolverDef.body = m_body; + + b3SoftBodyForceSolver forceSolver(forceSolverDef); + + forceSolver.Solve(dt, gravity); + } + + // Copy node state to state buffer + b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_mesh->vertexCount * sizeof(b3Vec3)); + b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_mesh->vertexCount * sizeof(b3Vec3)); + + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + positions[i] = m_nodes[i].m_position; + velocities[i] = m_nodes[i].m_velocity; + } + + { + // Solve constraints + b3SoftBodyContactSolverDef contactSolverDef; + contactSolverDef.allocator = m_allocator; + contactSolverDef.positions = positions; + contactSolverDef.velocities = velocities; + contactSolverDef.bodyContactCount = m_bodyContactCount; + contactSolverDef.bodyContacts = m_bodyContacts; + + b3SoftBodyContactSolver contactSolver(contactSolverDef); + + { + // Inititalize constraints + contactSolver.InitializeBodyContactConstraints(); + } + + { + // Warm-start velocity constraint solver + contactSolver.WarmStart(); + } + + { + // Solve velocity constraints + for (u32 i = 0; i < velocityIterations; ++i) + { + contactSolver.SolveBodyContactVelocityConstraints(); + } + } + + { + // Cache impulses for warm-starting + contactSolver.StoreImpulses(); + } + + // Integrate velocities + float32 h = dt; + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + positions[i] += h * velocities[i]; + } + + // 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_bodyContactCount; ++i) + { + b3Body* body = m_bodyContacts[i]->m_s2->GetBody(); + + if (body->GetType() == e_staticBody) + { + continue; + } + + body->SynchronizeTransform(); + + body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation); + + body->SynchronizeShapes(); + } + } + + // Copy state buffers back to the nodes + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + m_nodes[i].m_position = positions[i]; + m_nodes[i].m_velocity = velocities[i]; + } + + m_allocator->Free(velocities); + m_allocator->Free(positions); +} \ No newline at end of file diff --git a/test/ignore.txt b/test/ignore.txt deleted file mode 100644 index e69de29..0000000