diff --git a/examples/testbed/framework/cloth_dragger.cpp b/examples/testbed/framework/cloth_dragger.cpp index fb208c2..40467c9 100644 --- a/examples/testbed/framework/cloth_dragger.cpp +++ b/examples/testbed/framework/cloth_dragger.cpp @@ -20,10 +20,12 @@ b3ClothDragger::b3ClothDragger(b3Ray3* ray, b3Cloth* cloth) { - m_spring = false; + m_staticDrag = true; m_ray = ray; m_cloth = cloth; m_triangle = nullptr; + m_km = 10000.0f; + m_kd = 0.0f; } b3ClothDragger::~b3ClothDragger() @@ -42,7 +44,8 @@ bool b3ClothDragger::StartDragging() } m_mesh = m_cloth->GetMesh(); - m_triangle = m_mesh->triangles + rayOut.triangle; + m_triangleIndex = rayOut.triangle; + m_triangle = m_mesh->triangles + m_triangleIndex; m_x = rayOut.fraction; b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1); @@ -68,42 +71,7 @@ bool b3ClothDragger::StartDragging() 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 + if (m_staticDrag) { m_t1 = p1->GetType(); p1->SetType(e_staticParticle); @@ -114,6 +82,27 @@ bool b3ClothDragger::StartDragging() 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; } @@ -127,11 +116,7 @@ void b3ClothDragger::Drag() b3Vec3 dx = B - A; - if (m_spring) - { - m_particle->SetPosition(B); - } - else + if (m_staticDrag) { b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1); p1->ApplyTranslation(dx); @@ -142,11 +127,15 @@ void b3ClothDragger::Drag() b3Particle* p3 = m_cloth->GetParticle(m_triangle->v3); p3->ApplyTranslation(dx); } + else + { + m_particle->SetPosition(B); + } } -void b3ClothDragger::SetSpring(bool bit) +void b3ClothDragger::SetStaticDrag(bool bit) { - if (bit == m_spring) + if (bit == m_staticDrag) { return; } @@ -156,26 +145,24 @@ void b3ClothDragger::SetSpring(bool bit) StopDragging(); } - m_spring = bit; + m_staticDrag = bit; } void b3ClothDragger::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 + 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; } diff --git a/examples/testbed/framework/cloth_dragger.h b/examples/testbed/framework/cloth_dragger.h index 6bbb797..9661570 100644 --- a/examples/testbed/framework/cloth_dragger.h +++ b/examples/testbed/framework/cloth_dragger.h @@ -23,7 +23,8 @@ #include #include #include -#include +#include +#include // A cloth triangle dragger. class b3ClothDragger @@ -32,10 +33,18 @@ public: b3ClothDragger(b3Ray3* ray, b3Cloth* cloth); ~b3ClothDragger(); - void SetSpring(bool bit); + void SetStaticDrag(bool bit); - bool GetSpring() const; + bool GetStaticDrag() const; + void SetMouseStiffness(float32 k); + + float32 GetMouseStiffness(); + + void SetMouseDamping(float32 k); + + float32 GetMouseDamping(); + bool IsDragging() const; bool StartDragging(); @@ -53,22 +62,42 @@ private: b3Cloth* m_cloth; const b3ClothMesh* m_mesh; + u32 m_triangleIndex; b3ClothMeshTriangle* m_triangle; float32 m_u, m_v; - bool m_spring; - + float32 m_km; + float32 m_kd; b3Particle* m_particle; - b3SpringForce* m_s1; - b3SpringForce* m_s2; - b3SpringForce* m_s3; + b3MouseForce* m_mf; + bool m_staticDrag; b3ParticleType m_t1, m_t2, m_t3; }; -inline bool b3ClothDragger::GetSpring() const +inline bool b3ClothDragger::GetStaticDrag() const { - return m_spring; + 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 diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 1fdc46f..58e0faa 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -68,9 +68,11 @@ #include #include #include -#include -#include -#include + +#include +#include +#include +#include #include #include diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index b4fda69..9419b23 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -163,6 +163,7 @@ private: friend class b3ShearForce; friend class b3StrechForce; friend class b3SpringForce; + friend class b3MouseForce; friend class b3ClothContactManager; // Compute mass of each particle. diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h index 03750d5..4b3f4aa 100644 --- a/include/bounce/cloth/cloth_triangle.h +++ b/include/bounce/cloth/cloth_triangle.h @@ -44,6 +44,7 @@ private: friend class b3Particle; friend class b3ShearForce; friend class b3StrechForce; + friend class b3MouseForce; friend class b3ClothContactManager; friend class b3ParticleTriangleContact; friend class b3ClothSolver; diff --git a/include/bounce/cloth/force.h b/include/bounce/cloth/forces/force.h similarity index 98% rename from include/bounce/cloth/force.h rename to include/bounce/cloth/forces/force.h index 3fb51cd..27873f6 100644 --- a/include/bounce/cloth/force.h +++ b/include/bounce/cloth/forces/force.h @@ -32,6 +32,7 @@ enum b3ForceType e_strechForce, e_shearForce, e_springForce, + e_mouseForce, }; struct b3ForceDef 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/shear_force.h b/include/bounce/cloth/forces/shear_force.h similarity index 98% rename from include/bounce/cloth/shear_force.h rename to include/bounce/cloth/forces/shear_force.h index 8485795..9092100 100644 --- a/include/bounce/cloth/shear_force.h +++ b/include/bounce/cloth/forces/shear_force.h @@ -19,7 +19,7 @@ #ifndef B3_SHEAR_FORCE_H #define B3_SHEAR_FORCE_H -#include +#include class b3ClothTriangle; diff --git a/include/bounce/cloth/spring_force.h b/include/bounce/cloth/forces/spring_force.h similarity index 98% rename from include/bounce/cloth/spring_force.h rename to include/bounce/cloth/forces/spring_force.h index fa01405..3cd0a41 100644 --- a/include/bounce/cloth/spring_force.h +++ b/include/bounce/cloth/forces/spring_force.h @@ -19,7 +19,7 @@ #ifndef B3_SPRING_FORCE_H #define B3_SPRING_FORCE_H -#include +#include struct b3SpringForceDef : public b3ForceDef { diff --git a/include/bounce/cloth/strech_force.h b/include/bounce/cloth/forces/strech_force.h similarity index 98% rename from include/bounce/cloth/strech_force.h rename to include/bounce/cloth/forces/strech_force.h index b183c05..7d8dde9 100644 --- a/include/bounce/cloth/strech_force.h +++ b/include/bounce/cloth/forces/strech_force.h @@ -19,7 +19,7 @@ #ifndef B3_STRECH_FORCE_H #define B3_STRECH_FORCE_H -#include +#include class b3ClothTriangle; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index 4de14cb..a4d9aef 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -123,6 +123,7 @@ private: friend class b3StrechForce; friend class b3ShearForce; friend class b3SpringForce; + friend class b3MouseForce; b3Particle(const b3ParticleDef& def, b3Cloth* cloth); ~b3Particle(); diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index e45cd81..b5f957b 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -20,10 +20,9 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/bounce/cloth/cloth_force_solver.cpp b/src/bounce/cloth/cloth_force_solver.cpp index 2fb3953..c31aaab 100644 --- a/src/bounce/cloth/cloth_force_solver.cpp +++ b/src/bounce/cloth/cloth_force_solver.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/bounce/cloth/force.cpp b/src/bounce/cloth/forces/force.cpp similarity index 80% rename from src/bounce/cloth/force.cpp rename to src/bounce/cloth/forces/force.cpp index 4de3bf5..c1368b5 100644 --- a/src/bounce/cloth/force.cpp +++ b/src/bounce/cloth/forces/force.cpp @@ -16,10 +16,11 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include -#include +#include +#include +#include +#include +#include b3Force* b3Force::Create(const b3ForceDef* def) { @@ -44,6 +45,12 @@ b3Force* b3Force::Create(const b3ForceDef* def) force = new (block) b3SpringForce((b3SpringForceDef*)def); break; } + case e_mouseForce: + { + void* block = b3Alloc(sizeof(b3MouseForce)); + force = new (block) b3MouseForce((b3MouseForceDef*)def); + break; + } default: { B3_ASSERT(false); @@ -81,6 +88,13 @@ void b3Force::Destroy(b3Force* force) b3Free(force); break; } + case e_mouseForce: + { + b3MouseForce* o = (b3MouseForce*)force; + o->~b3MouseForce(); + b3Free(force); + break; + } default: { B3_ASSERT(false); diff --git a/src/bounce/cloth/forces/mouse_force.cpp b/src/bounce/cloth/forces/mouse_force.cpp new file mode 100644 index 0000000..c15b01b --- /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]; + + // Jacobian + b3Mat33 J[4][4]; + for (u32 i = 0; i < 4; ++i) + { + for (u32 j = 0; j < 4; ++j) + { + //b3Mat33 d2Cvxij; + //b3Mat33 Jij = -m_km * (b3Outer(dCdx[i], dCdx[j]) + C * d2Cvxij); + b3Mat33 Jij = -m_km * b3Outer(dCdx[i], dCdx[j]); + + J[i][j] = Jij; + } + } + + dfdx(i1, i1) += J[0][0]; + dfdx(i1, i2) += J[0][1]; + dfdx(i1, i3) += J[0][2]; + dfdx(i1, i4) += J[0][3]; + + dfdx(i2, i1) += J[1][0]; + dfdx(i2, i2) += J[1][1]; + dfdx(i2, i3) += J[1][2]; + dfdx(i2, i4) += J[1][3]; + + dfdx(i3, i1) += J[2][0]; + dfdx(i3, i2) += J[2][1]; + dfdx(i3, i3) += J[2][2]; + dfdx(i3, i4) += J[2][3]; + + dfdx(i4, i1) += J[3][0]; + dfdx(i4, i2) += J[3][1]; + dfdx(i4, i3) += J[3][2]; + dfdx(i4, i4) += J[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]; + + // Jacobian + b3Mat33 J[4][4]; + for (u32 i = 0; i < 4; ++i) + { + for (u32 j = 0; j < 4; ++j) + { + b3Mat33 Jij = -m_kd * b3Outer(dCdx[i], dCdx[j]); + + J[i][j] = Jij; + } + } + + dfdv(i1, i1) += J[0][0]; + dfdv(i1, i2) += J[0][1]; + dfdv(i1, i3) += J[0][2]; + dfdv(i1, i4) += J[0][3]; + + dfdv(i2, i1) += J[1][0]; + dfdv(i2, i2) += J[1][1]; + dfdv(i2, i3) += J[1][2]; + dfdv(i2, i4) += J[1][3]; + + dfdv(i3, i1) += J[2][0]; + dfdv(i3, i2) += J[2][1]; + dfdv(i3, i3) += J[2][2]; + dfdv(i3, i4) += J[2][3]; + + dfdv(i4, i1) += J[3][0]; + dfdv(i4, i2) += J[3][1]; + dfdv(i4, i3) += J[3][2]; + dfdv(i4, i4) += J[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/shear_force.cpp b/src/bounce/cloth/forces/shear_force.cpp similarity index 99% rename from src/bounce/cloth/shear_force.cpp rename to src/bounce/cloth/forces/shear_force.cpp index a6c28a7..a94f835 100644 --- a/src/bounce/cloth/shear_force.cpp +++ b/src/bounce/cloth/forces/shear_force.cpp @@ -16,7 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#include #include #include #include diff --git a/src/bounce/cloth/spring_force.cpp b/src/bounce/cloth/forces/spring_force.cpp similarity index 98% rename from src/bounce/cloth/spring_force.cpp rename to src/bounce/cloth/forces/spring_force.cpp index 8248c6d..4c365a1 100644 --- a/src/bounce/cloth/spring_force.cpp +++ b/src/bounce/cloth/forces/spring_force.cpp @@ -16,7 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#include #include #include #include diff --git a/src/bounce/cloth/strech_force.cpp b/src/bounce/cloth/forces/strech_force.cpp similarity index 99% rename from src/bounce/cloth/strech_force.cpp rename to src/bounce/cloth/forces/strech_force.cpp index e918809..0b87bdc 100644 --- a/src/bounce/cloth/strech_force.cpp +++ b/src/bounce/cloth/forces/strech_force.cpp @@ -16,7 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include +#include #include #include #include